diff --git a/.devcontainer/README.md b/.devcontainer/README.md new file mode 100644 index 000000000..c7f0646d2 --- /dev/null +++ b/.devcontainer/README.md @@ -0,0 +1,26 @@ +# CARMA Streets Dev Container Setup +## Introduction +This is the setup for the VSCode extension [devcontainer](https://code.visualstudio.com/docs/devcontainers/containers) extension, which allows developers to standup a containerized development environment with the VS Code IDE. The current configuration includes the installation of common VSCode plugins for C++, CMake, Python, Mark Down and more. A complete list of VS Code installed plugins can be found in the `./devcontainer/devcontainer.json` under `"customizations.vscode.extensions` attribute. The setup also stands up the CARMA Streets [docker compose file](../docker-compose.yml) , which provides your development environment with easy access to Kafka and CARMA Streets services for integration testing of functionality. +## Suggested Structure of CARMA Street Service +```bash +├── +│ ├── src/ # Source Code +│ ├── include/ # Headers +│ ├── manifest.json # CARMA Streets Service configuration file +│ ├── CMakeLists.txt # CMake build file +│ ├── README.md # Documentation about Service +│ ├── build.sh # Build script to build streets_util libraries for service and service itself +| ├── install_dependencies.sh # Optional file to install external dependencies not included in base image +│ └── Dockerfile # Dockerfile to build streets serivce +``` +## Setup for new CARMA Streets Service +* Select a streets base image ([streets base images](../README.md#base-images)) and uncomment appropriate lines in `.devcontainer/devcontainer.json` and `.devcontainer/docker-compose-devcontainer.yml` (See comments in files) +* Use ctrl-shift-P (Command Pallette) and select *Dev Containers: Open Folder in Container*. +* Use VS Code terminals to build new streets service, preferably using a `build.sh` script under your new service directory(see [Sensor Data Sharing Service](../sensor_data_sharing_service/README.md)). +## Setup for existing CARMA Streets Service +* Select a streets base image ([streets base images](../README.md#base-images)) and uncomment appropriate lines in `.devcontainer/devcontainer.json` and `.devcontainer/docker-compose-devcontainer.yml` (See comments in files) +* Comment out existing service inside `docker-compose.yml` +* Use ctrl-shift-P (Command Pallette) and select *Dev Containers: Open Folder in Container*. +* Use VS Code terminals to build existing streets service, preferably using a `build.sh` script under your new service directory(see [Sensor Data Sharing Service](../sensor_data_sharing_service/README.md)). + + diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 000000000..e0c683c73 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,42 @@ +{ + "name": "Existing Docker Compose (Extend)", + // First "../docker-compose.yml launches all of CARMA Street including Kafka, V2X-Hub and existing CARMA Streets service + // Second "docker-compose-devcontainer.yml" lauches dev container for developing and integration testing new and existing + // CARMA Streets services + "dockerComposeFile": [ + "../docker-compose.yml", + "docker-compose-devcontainer.yml" + ], + + // Uncomment for Streets Service Base dev environment + "service": "streets_service_base", + // Uncomment for Streets Service Base Lanelet aware dev environment + // "service": "streets_service_base_lanelet_aware", + // Specify the services you want to run + "runServices": ["php", "v2xhub", "db", "zookeeper", "kafka", "kowl"], + // The optional 'workspaceFolder' property is the path VS Code should open by default when + // connected. This is typically a file mount in .devcontainer/docker-compose.yml + "workspaceFolder": "/home/carma-streets/", + + // Uncomment for Sensor Data Sharing Service . + // "postCreateCommand": "/home/carma-streets/sensor_data_sharing_service/build.sh", + + // Configure tool-specific properties. + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools-extension-pack", + "github.vscode-github-actions", + "yzhang.markdown-all-in-one", + "ms-python.python", + "ms-python.black-formatter", + "ms-python.isort", + "streetsidesoftware.code-spell-checker", + "sonarsource.sonarlint-vscode", + "esbenp.prettier-vscode" + ] + } + }, + "shutdownAction": "stopCompose" + +} diff --git a/.devcontainer/docker-compose-devcontainer.yml b/.devcontainer/docker-compose-devcontainer.yml new file mode 100644 index 000000000..1e7681695 --- /dev/null +++ b/.devcontainer/docker-compose-devcontainer.yml @@ -0,0 +1,39 @@ +version: '3.7' +services: + # Uncomment for streets service base dev environment + streets_service_base: + # Replace bionic with distro of choice + image: usdotfhwastoldev/streets_service_base:bionic + # Remove build parameter to force pull from dockerhub + build: + context: . + dockerfile: ./streets_service_base/Dockerfile + args: + UBUNTU_CODENAME: bionic + network_mode: host + command: /bin/sh -c "while sleep 1000; do :; done" + + + # Uncomment for streets service base lanelet aware dev environment + streets_service_base_lanelet_aware: + image: usdotfhwastoldev/streets_service_base_lanelet_aware:bionic + # Remove build parameter to force pull from dockerhub + build: + context: . + dockerfile: ./streets_service_base_lanelet_aware/Dockerfile + network_mode: host + + + volumes: + # Update this to wherever you want VS Code to mount the folder of your project + - .:/home/carma-streets:cached + + # Uncomment the next four lines if you will use a ptrace-based debugger like C++, Go, and Rust. + # cap_add: + # - SYS_PTRACE + # security_opt: + # - seccomp:unconfined + + # Overrides default command so things don't shut down after the process ends. + command: /bin/sh -c "while sleep 1000; do :; done" + diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml deleted file mode 100644 index 3b71fdf10..000000000 --- a/.github/workflows/ci.yml +++ /dev/null @@ -1,154 +0,0 @@ -name: CI -on: - push: - pull_request: -jobs: - build: - defaults: - run: - shell: bash - runs-on: ubuntu-latest-8-cores - container: - image: ubuntu:jammy-20230126 - env: - DEBIAN_FRONTEND: noninteractive - INIT_ENV: "/home/carma-streets/.base-image/init-env.sh" - SONAR_SCANNER_VERSION: "4.6.2.2472" - TERM: xterm - options: "--user root" - steps: - # Bionic's git version is not sufficient for actions/checkout 0 fetch-depth, - # remove this step after rebasing carma-streets to newer Ubuntu release - - name: Install newer git for checkout - run: | - apt-get update - apt-get install -y software-properties-common - add-apt-repository -u ppa:git-core/ppa - apt-get install -y git - - name: Checkout ${{ github.event.repository.name }} - uses: actions/checkout@v3.3.0 - with: - path: ${{ github.event.repository.name }} - fetch-depth: 0 - - name: Move source code - run: mv $GITHUB_WORKSPACE/${{ github.event.repository.name }} /home/carma-streets - - name: Install dependencies - run: | - mkdir /home/carma-streets/.base-image - touch /home/carma-streets/.base-image/init-env.sh - cd /home/carma-streets/build_scripts - ./install_dependencies.sh - ./install_test_dependencies.sh - mkdir -p /home/carma-streets/ext - ./install_rest_server_dependencies.sh - - - name: Install librdkafka - run: | - cd /home/carma-streets/ext/ - git clone --depth 1 https://github.com/confluentinc/librdkafka.git /home/carma-streets/ext/librdkafka - cd /home/carma-streets/ext/librdkafka/ - cmake -H. -B_cmake_build - cmake --build _cmake_build - cmake --build _cmake_build --target install - - name: Install rapidjson - run: | - cd /home/carma-streets/ext/ - git clone --depth 1 https://github.com/Tencent/rapidjson /home/carma-streets/ext/rapidjson - mkdir -p /home/carma-streets/ext/rapidjson/build - cd /home/carma-streets/ext/rapidjson/build - cmake .. - make -j - make install - - name: Install net-snmp - run: | - cd /home/carma-streets/ext/ - apt-get install -y libperl-dev curl - curl -L -O http://sourceforge.net/projects/net-snmp/files/net-snmp/5.9.1/net-snmp-5.9.1.tar.gz - tar -xvzf /home/carma-streets/ext/net-snmp-5.9.1.tar.gz - cd net-snmp-5.9.1/ - ./configure --with-default-snmp-version="1" --with-sys-contact="@@no.where" --with-sys-location="Unknown" --with-logfile="/var/log/snmpd.log" --with-persistent-directory="/var/net-snmp" - make -j - make install - # - name: Install PROJ for coordinate transformations - # run: | - # git clone --depth 1 https://github.com/OSGeo/PROJ.git /home/carma-streets/PROJ --branch 6.2.1 - # cd /home/carma-streets/PROJ - # ./autogen.sh - # ./configure - # make -j - # make install - # - name: Download a cmake module for PROJ - # run: | - # cd /usr/share/cmake-3.10/Modules - # curl -O https://raw.githubusercontent.com/mloskot/cmake-modules/master/modules/FindPROJ4.cmake - # - name: Install ROS melodic - # run: | - # apt install -y lsb-release - # sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' - # curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc |apt-key add - - # apt-get update - # apt-get install -y ros-melodic-catkin - # cd /opt/ros/melodic/ - # ls -a - # mkdir -p /home/carma-streets/.base-image - # echo "source /opt/ros/melodic/setup.bash" > "$INIT_ENV" - # - name: Install carma_lanelet2 - # run: | - # mkdir -p /home/carma-streets/carma_lanelet2/src - # cd /home/carma-streets/carma_lanelet2/src - # git init - # echo "temp" - # git remote add origin -f https://github.com/usdot-fhwa-stol/autoware.ai.git - # git config core.sparsecheckout true - # echo "common/hardcoded_params/*" >> .git/info/sparse-checkout - # echo "common/lanelet2_extension/*" >> .git/info/sparse-checkout - # echo "lanelet2/*" >> .git/info/sparse-checkout - # echo "mrt_cmake_modules/*" >> .git/info/sparse-checkout - # git pull --depth 1 origin refactor_lanelet2_extension - # git checkout refactor_lanelet2_extension - # rm -r lanelet2/lanelet2_python - # rm -r lanelet2/lanelet2_examples - # cd /home/carma-streets/carma_lanelet2 - # source /opt/ros/melodic/setup.bash - # apt-get install -y libeigen3-dev python-rospkg - # ROS_VERSION=1 LANELET2_EXTENSION_LOGGER_TYPE=1 catkin_make install - # cd /home/carma-streets/carma_lanelet2/install/ - # ls -a - # echo "source /home/carma-streets/carma_lanelet2/install/setup.bash" >> "$INIT_ENV" - - name: Install Sonar - run: | - SONAR_DIR=/opt/sonarqube - mkdir $SONAR_DIR - curl -o $SONAR_DIR/sonar-scanner.zip https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-${SONAR_SCANNER_VERSION}-linux.zip - curl -o $SONAR_DIR/build-wrapper.zip https://sonarcloud.io/static/cpp/build-wrapper-linux-x86.zip - curl -sL https://deb.nodesource.com/setup_16.x |bash - - apt-get install -y nodejs unzip - cd $SONAR_DIR - for ZIP in *.zip; do - unzip "$ZIP" -d . - rm "$ZIP" - done - mv $(ls $SONAR_DIR |grep "sonar-scanner-") $SONAR_DIR/sonar-scanner/ - mv $(ls $SONAR_DIR |grep "build-wrapper-") $SONAR_DIR/build-wrapper/ - echo $SONAR_DIR/sonar-scanner/bin >> $GITHUB_PATH - echo $SONAR_DIR/build-wrapper >> $GITHUB_PATH - - name: Build - run: | - source ${INIT_ENV} - cd /home/carma-streets/ - build-wrapper-linux-x86-64 --out-dir /home/carma-streets/bw-output ./build.sh - - name: Tests - run: | - cd /home/carma-streets/ - ldconfig - ./coverage.sh - - name: Archive test results - uses: actions/upload-artifact@v3 - with: - name: Test Results - path: /home/carma-streets/test_results - - name: Run SonarScanner - uses: usdot-fhwa-stol/actions/sonar-scanner@main - with: - sonar-token: ${{ secrets.SONAR_TOKEN }} - working-dir: /home/carma-streets diff --git a/.github/workflows/develop.yml b/.github/workflows/develop.yml new file mode 100644 index 000000000..270f7a9c1 --- /dev/null +++ b/.github/workflows/develop.yml @@ -0,0 +1,134 @@ +name: develop +on: + push: + branches: [develop] +jobs: + docker-build: + strategy: + matrix: + include: + - ubuntu-codename: bionic + build_lanelet_aware: true + # Currently install_lanelet2_dependencies.sh script only works for bionic. Disabled lanelet_aware build on newer distributions pending script updates. + - ubuntu-codename: focal + build_lanelet_aware: false + # Currently install_lanelet2_dependencies.sh script only works for bionic. Disabled lanelet_aware build on newer distributions pending script updates. + - ubuntu-codename: jammy + build_lanelet_aware: false + runs-on: ubuntu-latest + steps: + - name: Log in to the Container registry + uses: docker/login-action@v2 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Docker Build Streets Service Base + uses: docker/build-push-action@v3 + with: + push: true + build-args: | + UBUNTU_CODENAME=${{ matrix.ubuntu-codename }} + tags: usdotfhwastol/streets_service_base:${{ matrix.ubuntu-codename }} + file: ./streets_service_base/Dockerfile + - name: Docker Build Streets Service Base Lanelet Aware + if: ${{ matrix.build_lanelet_aware }} + uses: docker/build-push-action@v3 + with: + push: true + tags: usdotfhwastol/streets_service_base_lanelet_aware:${{ matrix.ubuntu-codename }} + file: ./streets_service_base_lanelet_aware/Dockerfile + build: + needs: docker-build + defaults: + run: + shell: bash + runs-on: ubuntu-latest-8-cores + container: + image: usdotfhwastoldev/streets_service_base_lanelet_aware:bionic + env: + DEBIAN_FRONTEND: noninteractive + SONAR_SCANNER_VERSION: "5.0.1.3006" + TERM: xterm + options: "--user root" + steps: + # Bionic's git version is not sufficient for actions/checkout 0 fetch-depth, + # remove this step after rebasing carma-streets to newer Ubuntu release + - name: Install newer git for checkout + run: | + apt-get update + apt-get install -y software-properties-common + add-apt-repository -u ppa:git-core/ppa + apt-get install -y git + - name: Checkout ${{ github.event.repository.name }} + uses: actions/checkout@v3.3.0 + with: + path: ${{ github.event.repository.name }} + fetch-depth: 0 + - name: Move source code + run: mv $GITHUB_WORKSPACE/${{ github.event.repository.name }} /home/carma-streets + - name: Install dependencies + run: | + cd /home/carma-streets/build_scripts + ./install_test_dependencies.sh + mkdir -p /home/carma-streets/ext + ./install_rest_server_dependencies.sh + - name: Install net-snmp + run: | + cd /home/carma-streets/ext/ + apt-get install -y libperl-dev curl + curl -k -L -O http://sourceforge.net/projects/net-snmp/files/net-snmp/5.9.1/net-snmp-5.9.1.tar.gz + tar -xvzf /home/carma-streets/ext/net-snmp-5.9.1.tar.gz + cd net-snmp-5.9.1/ + ./configure --with-default-snmp-version="1" --with-sys-contact="@@no.where" --with-sys-location="Unknown" --with-logfile="/var/log/snmpd.log" --with-persistent-directory="/var/net-snmp" + make -j + make install + - name: Set up JDK 17 + uses: actions/setup-java@v3 # The setup-java action provides the functionality for GitHub Actions runners for Downloading and setting up a requested version of Java + with: + java-version: 17 + distribution: "temurin" + - name: Install Sonar + run: | + SONAR_DIR=/opt/sonarqube + mkdir $SONAR_DIR + curl -o $SONAR_DIR/sonar-scanner.zip https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-${SONAR_SCANNER_VERSION}-linux.zip + curl -o $SONAR_DIR/build-wrapper.zip https://sonarcloud.io/static/cpp/build-wrapper-linux-x86.zip + curl -sL https://deb.nodesource.com/setup_16.x | bash - + apt-get install -y nodejs unzip + # Set the JAVA_HOME to a compatible version of Java, e.g., Java 17 + export JAVA_HOME=$GITHUB_WORKSPACE/java-17 + cd $SONAR_DIR + for ZIP in *.zip; do + unzip "$ZIP" -d . + rm "$ZIP" + done + mv $(ls $SONAR_DIR | grep "sonar-scanner-") $SONAR_DIR/sonar-scanner/ + mv $(ls $SONAR_DIR | grep "build-wrapper-") $SONAR_DIR/build-wrapper/ + echo $SONAR_DIR/sonar-scanner/bin >> $GITHUB_PATH + echo $SONAR_DIR/build-wrapper >> $GITHUB_PATH + env: + JAVA_HOME: $GITHUB_WORKSPACE/java-17 + + - name: Check Java Version + run: | + java -version + echo $JAVA_HOME + - name: Build + run: | + cd /home/carma-streets/ + build-wrapper-linux-x86-64 --out-dir /home/carma-streets/bw-output ./build.sh + - name: Tests + run: | + cd /home/carma-streets/ + ldconfig + ./coverage.sh + - name: Archive test results + uses: actions/upload-artifact@v3 + with: + name: Test Results + path: /home/carma-streets/test_results + - name: Run SonarScanner + uses: usdot-fhwa-stol/actions/sonar-scanner@main + with: + sonar-token: ${{ secrets.SONAR_TOKEN }} + working-dir: /home/carma-streets diff --git a/.github/workflows/feature_branch.yml b/.github/workflows/feature_branch.yml new file mode 100644 index 000000000..08e2bbe6e --- /dev/null +++ b/.github/workflows/feature_branch.yml @@ -0,0 +1,99 @@ +name: feature_branch +on: + pull_request: + types: [opened, synchronize, reopened] +jobs: + build: + defaults: + run: + shell: bash + runs-on: ubuntu-latest-8-cores + container: + image: usdotfhwastoldev/streets_service_base_lanelet_aware:bionic + env: + DEBIAN_FRONTEND: noninteractive + SONAR_SCANNER_VERSION: "5.0.1.3006" + TERM: xterm + options: "--user root" + steps: + # Bionic's git version is not sufficient for actions/checkout 0 fetch-depth, + # remove this step after rebasing carma-streets to newer Ubuntu release + - name: Install newer git for checkout + run: | + apt-get update + apt-get install -y software-properties-common + add-apt-repository -u ppa:git-core/ppa + apt-get install -y git + - name: Checkout ${{ github.event.repository.name }} + uses: actions/checkout@v3.3.0 + with: + path: ${{ github.event.repository.name }} + fetch-depth: 0 + - name: Move source code + run: mv $GITHUB_WORKSPACE/${{ github.event.repository.name }} /home/carma-streets + - name: Install dependencies + run: | + cd /home/carma-streets/build_scripts + ./install_test_dependencies.sh + mkdir -p /home/carma-streets/ext + ./install_rest_server_dependencies.sh + - name: Install net-snmp + run: | + cd /home/carma-streets/ext/ + apt-get install -y libperl-dev curl + curl -k -L -O http://sourceforge.net/projects/net-snmp/files/net-snmp/5.9.1/net-snmp-5.9.1.tar.gz + tar -xvzf /home/carma-streets/ext/net-snmp-5.9.1.tar.gz + cd net-snmp-5.9.1/ + ./configure --with-default-snmp-version="1" --with-sys-contact="@@no.where" --with-sys-location="Unknown" --with-logfile="/var/log/snmpd.log" --with-persistent-directory="/var/net-snmp" + make -j + make install + - name: Set up JDK 17 + uses: actions/setup-java@v3 # The setup-java action provides the functionality for GitHub Actions runners for Downloading and setting up a requested version of Java + with: + java-version: 17 + distribution: "temurin" + - name: Install Sonar + run: | + SONAR_DIR=/opt/sonarqube + mkdir $SONAR_DIR + curl -o $SONAR_DIR/sonar-scanner.zip https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-${SONAR_SCANNER_VERSION}-linux.zip + curl -o $SONAR_DIR/build-wrapper.zip https://sonarcloud.io/static/cpp/build-wrapper-linux-x86.zip + curl -sL https://deb.nodesource.com/setup_16.x | bash - + apt-get install -y nodejs unzip + # Set the JAVA_HOME to a compatible version of Java, e.g., Java 17 + export JAVA_HOME=$GITHUB_WORKSPACE/java-17 + cd $SONAR_DIR + for ZIP in *.zip; do + unzip "$ZIP" -d . + rm "$ZIP" + done + mv $(ls $SONAR_DIR | grep "sonar-scanner-") $SONAR_DIR/sonar-scanner/ + mv $(ls $SONAR_DIR | grep "build-wrapper-") $SONAR_DIR/build-wrapper/ + echo $SONAR_DIR/sonar-scanner/bin >> $GITHUB_PATH + echo $SONAR_DIR/build-wrapper >> $GITHUB_PATH + env: + JAVA_HOME: $GITHUB_WORKSPACE/java-17 + + - name: Check Java Version + run: | + java -version + echo $JAVA_HOME + - name: Build + run: | + cd /home/carma-streets/ + build-wrapper-linux-x86-64 --out-dir /home/carma-streets/bw-output ./build.sh + - name: Tests + run: | + cd /home/carma-streets/ + ldconfig + ./coverage.sh + - name: Archive test results + uses: actions/upload-artifact@v3 + with: + name: Test Results + path: /home/carma-streets/test_results + - name: Run SonarScanner + uses: usdot-fhwa-stol/actions/sonar-scanner@main + with: + sonar-token: ${{ secrets.SONAR_TOKEN }} + working-dir: /home/carma-streets diff --git a/.github/workflows/master.yml b/.github/workflows/master.yml new file mode 100644 index 000000000..2630f267a --- /dev/null +++ b/.github/workflows/master.yml @@ -0,0 +1,134 @@ +name: master +on: + push: + branches: [master] +jobs: + docker-build: + strategy: + matrix: + include: + - ubuntu-codename: bionic + build_lanelet_aware: true + # Currently install_lanelet2_dependencies.sh script only works for bionic. Disabled lanelet_aware build on newer distributions pending script updates. + - ubuntu-codename: focal + build_lanelet_aware: false + # Currently install_lanelet2_dependencies.sh script only works for bionic. Disabled lanelet_aware build on newer distributions pending script updates. + - ubuntu-codename: jammy + build_lanelet_aware: false + runs-on: ubuntu-latest + steps: + - name: Log in to the Container registry + uses: docker/login-action@v2 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Docker Build Streets Service Base + uses: docker/build-push-action@v3 + with: + push: true + build-args: | + UBUNTU_CODENAME=${{ matrix.ubuntu-codename }} + tags: usdotfhwastol/streets_service_base:${{ matrix.ubuntu-codename }} + file: ./streets_service_base/Dockerfile + - name: Docker Build Streets Service Base Lanelet Aware + if: ${{ matrix.build_lanelet_aware }} + uses: docker/build-push-action@v3 + with: + push: true + tags: usdotfhwastol/streets_service_base_lanelet_aware:${{ matrix.ubuntu-codename }} + file: ./streets_service_base_lanelet_aware/Dockerfile + build: + needs: docker-build + defaults: + run: + shell: bash + runs-on: ubuntu-latest-8-cores + container: + image: usdotfhwastol/streets_service_base_lanelet_aware:bionic + env: + DEBIAN_FRONTEND: noninteractive + SONAR_SCANNER_VERSION: "5.0.1.3006" + TERM: xterm + options: "--user root" + steps: + # Bionic's git version is not sufficient for actions/checkout 0 fetch-depth, + # remove this step after rebasing carma-streets to newer Ubuntu release + - name: Install newer git for checkout + run: | + apt-get update + apt-get install -y software-properties-common + add-apt-repository -u ppa:git-core/ppa + apt-get install -y git + - name: Checkout ${{ github.event.repository.name }} + uses: actions/checkout@v3.3.0 + with: + path: ${{ github.event.repository.name }} + fetch-depth: 0 + - name: Move source code + run: mv $GITHUB_WORKSPACE/${{ github.event.repository.name }} /home/carma-streets + - name: Install dependencies + run: | + cd /home/carma-streets/build_scripts + ./install_test_dependencies.sh + mkdir -p /home/carma-streets/ext + ./install_rest_server_dependencies.sh + - name: Install net-snmp + run: | + cd /home/carma-streets/ext/ + apt-get install -y libperl-dev curl + curl -k -L -O http://sourceforge.net/projects/net-snmp/files/net-snmp/5.9.1/net-snmp-5.9.1.tar.gz + tar -xvzf /home/carma-streets/ext/net-snmp-5.9.1.tar.gz + cd net-snmp-5.9.1/ + ./configure --with-default-snmp-version="1" --with-sys-contact="@@no.where" --with-sys-location="Unknown" --with-logfile="/var/log/snmpd.log" --with-persistent-directory="/var/net-snmp" + make -j + make install + - name: Set up JDK 17 + uses: actions/setup-java@v3 # The setup-java action provides the functionality for GitHub Actions runners for Downloading and setting up a requested version of Java + with: + java-version: 17 + distribution: "temurin" + - name: Install Sonar + run: | + SONAR_DIR=/opt/sonarqube + mkdir $SONAR_DIR + curl -o $SONAR_DIR/sonar-scanner.zip https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-${SONAR_SCANNER_VERSION}-linux.zip + curl -o $SONAR_DIR/build-wrapper.zip https://sonarcloud.io/static/cpp/build-wrapper-linux-x86.zip + curl -sL https://deb.nodesource.com/setup_16.x | bash - + apt-get install -y nodejs unzip + # Set the JAVA_HOME to a compatible version of Java, e.g., Java 17 + export JAVA_HOME=$GITHUB_WORKSPACE/java-17 + cd $SONAR_DIR + for ZIP in *.zip; do + unzip "$ZIP" -d . + rm "$ZIP" + done + mv $(ls $SONAR_DIR | grep "sonar-scanner-") $SONAR_DIR/sonar-scanner/ + mv $(ls $SONAR_DIR | grep "build-wrapper-") $SONAR_DIR/build-wrapper/ + echo $SONAR_DIR/sonar-scanner/bin >> $GITHUB_PATH + echo $SONAR_DIR/build-wrapper >> $GITHUB_PATH + env: + JAVA_HOME: $GITHUB_WORKSPACE/java-17 + + - name: Check Java Version + run: | + java -version + echo $JAVA_HOME + - name: Build + run: | + cd /home/carma-streets/ + build-wrapper-linux-x86-64 --out-dir /home/carma-streets/bw-output ./build.sh + - name: Tests + run: | + cd /home/carma-streets/ + ldconfig + ./coverage.sh + - name: Archive test results + uses: actions/upload-artifact@v3 + with: + name: Test Results + path: /home/carma-streets/test_results + - name: Run SonarScanner + uses: usdot-fhwa-stol/actions/sonar-scanner@main + with: + sonar-token: ${{ secrets.SONAR_TOKEN }} + working-dir: /home/carma-streets diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 000000000..4e26c995e --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,135 @@ +name: release +on: + push: + tags: + - "carma-system-*" +jobs: + docker-build: + strategy: + matrix: + include: + - ubuntu-codename: bionic + build_lanelet_aware: true + # Currently install_lanelet2_dependencies.sh script only works for bionic. Disabled lanelet_aware build on newer distributions pending script updates. + - ubuntu-codename: focal + build_lanelet_aware: false + # Currently install_lanelet2_dependencies.sh script only works for bionic. Disabled lanelet_aware build on newer distributions pending script updates. + - ubuntu-codename: jammy + build_lanelet_aware: false + runs-on: ubuntu-latest + steps: + - name: Log in to the Container registry + uses: docker/login-action@v2 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Docker Build Streets Service Base + uses: docker/build-push-action@v3 + with: + push: true + build-args: | + UBUNTU_CODENAME=${{ matrix.ubuntu-codename }} + tags: usdotfhwastol/streets_service_base:${{ github.ref_name }}-${{ matrix.ubuntu-codename }} + file: ./streets_service_base/Dockerfile + - name: Docker Build Streets Service Base Lanelet Aware + if: ${{ matrix.build_lanelet_aware }} + uses: docker/build-push-action@v3 + with: + push: true + tags: usdotfhwastol/streets_service_base_lanelet_aware:${{ github.ref_name }}-${{ matrix.ubuntu-codename }} + file: ./streets_service_base_lanelet_aware/Dockerfile + build: + needs: docker-build + defaults: + run: + shell: bash + runs-on: ubuntu-latest-8-cores + container: + image: usdotfhwastol/streets_service_base_lanelet_aware:${{ github.ref_name }}-bionic + env: + DEBIAN_FRONTEND: noninteractive + SONAR_SCANNER_VERSION: "5.0.1.3006" + TERM: xterm + options: "--user root" + steps: + # Bionic's git version is not sufficient for actions/checkout 0 fetch-depth, + # remove this step after rebasing carma-streets to newer Ubuntu release + - name: Install newer git for checkout + run: | + apt-get update + apt-get install -y software-properties-common + add-apt-repository -u ppa:git-core/ppa + apt-get install -y git + - name: Checkout ${{ github.event.repository.name }} + uses: actions/checkout@v3.3.0 + with: + path: ${{ github.event.repository.name }} + fetch-depth: 0 + - name: Move source code + run: mv $GITHUB_WORKSPACE/${{ github.event.repository.name }} /home/carma-streets + - name: Install dependencies + run: | + cd /home/carma-streets/build_scripts + ./install_test_dependencies.sh + mkdir -p /home/carma-streets/ext + ./install_rest_server_dependencies.sh + - name: Install net-snmp + run: | + cd /home/carma-streets/ext/ + apt-get install -y libperl-dev curl + curl -k -L -O http://sourceforge.net/projects/net-snmp/files/net-snmp/5.9.1/net-snmp-5.9.1.tar.gz + tar -xvzf /home/carma-streets/ext/net-snmp-5.9.1.tar.gz + cd net-snmp-5.9.1/ + ./configure --with-default-snmp-version="1" --with-sys-contact="@@no.where" --with-sys-location="Unknown" --with-logfile="/var/log/snmpd.log" --with-persistent-directory="/var/net-snmp" + make -j + make install + - name: Set up JDK 17 + uses: actions/setup-java@v3 # The setup-java action provides the functionality for GitHub Actions runners for Downloading and setting up a requested version of Java + with: + java-version: 17 + distribution: "temurin" + - name: Install Sonar + run: | + SONAR_DIR=/opt/sonarqube + mkdir $SONAR_DIR + curl -o $SONAR_DIR/sonar-scanner.zip https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-${SONAR_SCANNER_VERSION}-linux.zip + curl -o $SONAR_DIR/build-wrapper.zip https://sonarcloud.io/static/cpp/build-wrapper-linux-x86.zip + curl -sL https://deb.nodesource.com/setup_16.x | bash - + apt-get install -y nodejs unzip + # Set the JAVA_HOME to a compatible version of Java, e.g., Java 17 + export JAVA_HOME=$GITHUB_WORKSPACE/java-17 + cd $SONAR_DIR + for ZIP in *.zip; do + unzip "$ZIP" -d . + rm "$ZIP" + done + mv $(ls $SONAR_DIR | grep "sonar-scanner-") $SONAR_DIR/sonar-scanner/ + mv $(ls $SONAR_DIR | grep "build-wrapper-") $SONAR_DIR/build-wrapper/ + echo $SONAR_DIR/sonar-scanner/bin >> $GITHUB_PATH + echo $SONAR_DIR/build-wrapper >> $GITHUB_PATH + env: + JAVA_HOME: $GITHUB_WORKSPACE/java-17 + + - name: Check Java Version + run: | + java -version + echo $JAVA_HOME + - name: Build + run: | + cd /home/carma-streets/ + build-wrapper-linux-x86-64 --out-dir /home/carma-streets/bw-output ./build.sh + - name: Tests + run: | + cd /home/carma-streets/ + ldconfig + ./coverage.sh + - name: Archive test results + uses: actions/upload-artifact@v3 + with: + name: Test Results + path: /home/carma-streets/test_results + - name: Run SonarScanner + uses: usdot-fhwa-stol/actions/sonar-scanner@main + with: + sonar-token: ${{ secrets.SONAR_TOKEN }} + working-dir: /home/carma-streets diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index c19b7d030..11ed0cffc 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -32,10 +32,15 @@ sonar.coverageReportPaths= /home/carma-streets/kafka_clients/coverage/coverage.x /home/carma-streets/tsc_client_service/coverage/coverage.xml, \ /home/carma-streets/streets_utils/streets_vehicle_scheduler/coverage/coverage.xml, \ /home/carma-streets/streets_utils/streets_desired_phase_plan/coverage/coverage.xml, \ -/home/carma-streets/streets_utils/streets_signal_optimization/coverage/coverage.xml - -# TODO : /home/carma-streets/intersection_model/coverage/coverage.xml, \ -# TODO: /home/carma-streets/message_services/coverage/coverage.xml, \ +/home/carma-streets/streets_utils/streets_phase_control_schedule/coverage/coverage.xml, \ +/home/carma-streets/streets_utils/streets_timing_plan/coverage/coverage.xml, \ +/home/carma-streets/streets_utils/streets_signal_optimization/coverage/coverage.xml, \ +/home/carma-streets/streets_utils/streets_snmp_cmd/coverage/coverage.xml, \ +/home/carma-streets/streets_utils/streets_messages/coverage/coverage.xml, \ +/home/carma-streets/streets_utils/json_utils/coverage/coverage.xml, \ +/home/carma-streets/intersection_model/coverage/coverage.xml, \ +/home/carma-streets/message_services/coverage/coverage.xml, \ +/home/carma-streets/sensor_data_sharing_service/coverage/coverage.xml #Encoding of the source code. Default is default system encoding sonar.sourceEncoding=UTF-8 @@ -45,7 +50,7 @@ sonar.scm.disabled=false sonar.scm.enabled=true sonar.scm.provider=git -sonar.cpp.file.suffixes=.cpp,.h,.tpp +sonar.cpp.file.suffixes=.cpp,.h,.hpp,.tpp #sonar.objc.file.suffixes=.h,.m,.mm sonar.c.file.suffixes=- #This is the name and version displayed in the SonarCloud UI. @@ -66,20 +71,26 @@ streets_service_configuration, \ streets_service_base, \ streets_vehicle_list, \ streets_signal_phase_and_timing, \ +streets_phase_control_schedule, \ +streets_timing_plan, \ streets_tsc_configuration, \ tsc_client_service, \ streets_vehicle_scheduler, \ streets_desired_phase_plan, \ -streets_signal_optimization +streets_signal_optimization, \ +streets_snmp_cmd, \ +streets_messages, \ +json_utils, \ +message_services, \ +intersection_model, \ +sensor_data_sharing_service -# TODO message_services -# TODO intersection_model kafka_clients.sonar.projectBaseDir=/home/carma-streets/kafka_clients/ scheduling_service.sonar.projectBaseDir=/home/carma-streets/scheduling_service -# message_services.sonar.projectBaseDir=/home/carma-streets/message_services/ +message_services.sonar.projectBaseDir=/home/carma-streets/message_services/ signal_opt_service.sonar.projectBaseDir=/home/carma-streets/signal_opt_service/ -# intersection_model.sonar.projectBaseDir=/home/carma-streets/intersection_model/ +intersection_model.sonar.projectBaseDir=/home/carma-streets/intersection_model/ streets_service_configuration.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_service_configuration streets_service_base.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_service_base streets_vehicle_list.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_vehicle_list @@ -89,6 +100,13 @@ streets_signal_phase_and_timing.sonar.projectBaseDir=/home/carma-streets/streets streets_tsc_configuration.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_tsc_configuration streets_desired_phase_plan.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_desired_phase_plan streets_signal_optimization.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_signal_optimization +streets_phase_control_schedule.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_phase_control_schedule +streets_timing_plan.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_timing_plan +streets_snmp_cmd.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_snmp_cmd +streets_messages.sonar.projectBaseDir=/home/carma-streets/streets_utils/streets_messages +json_utils.sonar.projectBaseDir=/home/carma-streets/streets_utils/json_utils +sensor_data_sharing_service.sonar.projectBaseDir=/home/carma-streets/sensor_data_sharing_service + @@ -98,12 +116,12 @@ kafka_clients.sonar.sources =src/,include/ kafka_clients.sonar.exclusions =test/** scheduling_service.sonar.sources =src/,include/ scheduling_service.sonar.exclusions =test/** -# message_services.sonar.sources =src/,include/,lib/ -# message_services.sonar.exclusions =test/** +message_services.sonar.sources =src/,include/,lib/ +message_services.sonar.exclusions =test/** signal_opt_service.sonar.sources =src/,include/ signal_opt_service.sonar.exclusions =test/** -# intersection_model.sonar.sources =src/,include/ -# intersection_model.sonar.exclusions =src/server/**,test/** +intersection_model.sonar.sources =src/,include/ +intersection_model.sonar.exclusions =src/server/**,test/** streets_service_base.sonar.sources =src/,include/ streets_service_base.sonar.exclusions =test/** streets_service_configuration.sonar.sources =src/,include/ @@ -122,15 +140,25 @@ streets_desired_phase_plan.sonar.sources =src/,include/ streets_desired_phase_plan.sonar.exclusions =test/** streets_signal_optimization.sonar.sources =src/,include/ streets_signal_optimization.sonar.exclusions =test/** - - +streets_phase_control_schedule.sonar.sources =src/,include/ +streets_phase_control_schedule.sonar.exclusions =test/** +streets_timing_plan.sonar.sources =src/,include/ +streets_timing_plan.sonar.exclusions =test/** +streets_snmp_cmd.sonar.sources =src/,include/ +streets_snmp_cmd.sonar.exclusions =test/** +streets_messages.sonar.sources =src/,include/ +streets_messages.sonar.exclusions =test/** +json_utils.sonar.sources =src/,include/ +json_utils.sonar.exclusions =test/** +sensor_data_sharing_service.sonar.sources =src/,include/ +sensor_data_sharing_service.sonar.exclusions =test/** #Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. kafka_clients.sonar.tests=test/ scheduling_service.sonar.tests=test/ -# message_services.sonar.tests=test/ +message_services.sonar.tests=test/ signal_opt_service.sonar.tests=test/ -# intersection_model.sonar.tests=test/ +intersection_model.sonar.tests=test/ streets_service_configuration.sonar.tests=test/ streets_service_base.sonar.tests=test/ streets_vehicle_list.sonar.tests=test/ @@ -140,5 +168,11 @@ streets_signal_phase_and_timing.sonar.tests=test/ streets_tsc_configuration.sonar.tests=test/ streets_desired_phase_plan.sonar.tests=test/ streets_signal_optimization.sonar.tests=test/ +streets_phase_control_schedule.sonar.tests=test/ +streets_timing_plan.sonar.tests=test/ +streets_snmp_cmd.sonar.tests=test/ +streets_messages.sonar.tests=test/ +json_utils.sonar.tests=test/ +sensor_data_sharing_service.sonar.tests=test/ diff --git a/README.md b/README.md index 6e3fa2dc9..1deb67120 100644 --- a/README.md +++ b/README.md @@ -6,13 +6,15 @@ |-----|-----|-----|-----|-----| [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/scheduling_service?label=scheduling%20service)](https://hub.docker.com/repository/docker/usdotfhwastol/scheduling_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/message_services?label=message%20services)](https://hub.docker.com/repository/docker/usdotfhwastol/message_services) | [ ![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/intersection_model?label=intersection%20model)](https://hub.docker.com/repository/docker/usdotfhwastol/intersection_model) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/signal_opt_service?label=signal%20opt%20service)](https://hub.docker.com/repository/docker/usdotfhwastol/signal_opt_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/tsc_service?label=tsc%20service&logoColor=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastol/tsc_service) | # DockerHub Release Candidate Builds -| Scheduling Service | Message Services | Intersection Model | Signal Opt Service | Tsc Service | -|----|----|----|----|----| -[![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/scheduling_service?label=tsc%20service&logoColor=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/scheduling_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/message_services?label=message%20services)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/message_services) | [ ![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/intersection_model?label=intersection%20model)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/intersection_model) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/signal_opt_service?label=signal%20opt%20service)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/signal_opt_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/tsc_service?label=tsc%20service&logoColor=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/tsc_service) | +| Scheduling Service | Message Services | Intersection Model | Signal Opt Service | Tsc Service | Sensor Data Sharing Service +|----|----|----|----|----|----| +[![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/scheduling_service?label=tsc%20service&logoColor=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/scheduling_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/message_services?label=message%20services)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/message_services) | [ ![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/intersection_model?label=intersection%20model)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/intersection_model) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/signal_opt_service?label=signal%20opt%20service)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/signal_opt_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/tsc_service?label=tsc%20service&logoColor=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/tsc_service) | ![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/sensor_data_sharing_service?label=sensor_data_sharing_%20service&logoColor=%232496ED)| # DockerHub Develop Builds -| Scheduling Service | Message Services | Intersection Model | Signal Opt Service | Tsc Service | -|-----|-----|-----|-----|-----| -[![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/scheduling_service?label=scheduling%20service)](https://hub.docker.com/repository/docker/usdotfhwastoldev/scheduling_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/message_services?label=message%20services)](https://hub.docker.com/repository/docker/usdotfhwastoldev/message_services) | [ ![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/intersection_model?label=intersection%20model)](https://hub.docker.com/repository/docker/usdotfhwastoldev/intersection_model) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/signal_opt_service?label=signal%20opt%20service)](https://hub.docker.com/repository/docker/usdotfhwastoldev/signal_opt_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/tsc_service?label=tsc%20service&logoColor=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastoldev/tsc_service) | +| Scheduling Service | Message Services | Intersection Model | Signal Opt Service | Tsc Service | Sensor Data Sharing Service +|-----|-----|-----|-----|-----|-----| +[![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/scheduling_service?label=scheduling%20service)](https://hub.docker.com/repository/docker/usdotfhwastoldev/scheduling_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/message_services?label=message%20services)](https://hub.docker.com/repository/docker/usdotfhwastoldev/message_services) | [ ![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/intersection_model?label=intersection%20model)](https://hub.docker.com/repository/docker/usdotfhwastoldev/intersection_model) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/signal_opt_service?label=signal%20opt%20service)](https://hub.docker.com/repository/docker/usdotfhwastoldev/signal_opt_service) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/tsc_service?label=tsc%20service&logoColor=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastoldev/tsc_service) | ![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/sensor_data_sharing_service?label=sensor_data_sharing_%20service&logoColor=%232496ED)| +# CARMA Streets Base Image Builds +[![Build and Push Base Images](https://github.com/usdot-fhwa-stol/carma-streets/actions/workflows/build_streets_base_images.yml/badge.svg)](https://github.com/usdot-fhwa-stol/carma-streets/actions/workflows/build_streets_base_images.yml) CARMA Streets is a component of CARMA ecosystem, which enables such a coordination among different transportation users. This component provides an interface for CDA participants to interact with the road infrastructure. CARMA Streets is also an edge-computing unit that improves the efficiency and performance of the Transportation Systems Management and Operations (TSMO) strategies. @@ -20,12 +22,59 @@ CARMA Streets is a component of CARMA ecosystem, which enables such a coordinati CARMA Streets architecture is based on a scalable services and layered architecture pattern that allows for easy deployment. Service components are packaged to contain one or more modules (classes) that represent a specific reusable function (e.g., decode a particular ASN.1 message) or an independently deployable business function (e.g., control interface to a signal controller). Services interact with each other via lightweight messaging service (e.g., Kafka) which allows for them be deployed either together or distributed for scalability and performance. A high-level abstract view of the architecture to communicate the design pattern is shown in Upcoming Figure. A more detailed Unified Modeling Language class and packaging diagrams to define the interfaces between services and layers and their interactions will be developed and documented here during implementation following an Agile Development Methodology. ## Deployment -Docker is the primary deployment mechanism to containerize one or more services. The CARMA Streets application and other major frameworks such as Kafka will run in their own separate containers. This document will be updated with a detailed Docker deployment strategy during later design phases. +Docker is the primary deployment mechanism to containerize one or more services. The CARMA Streets application and other major frameworks such as Kafka will run in their own separate containers. This document will be updated with a detailed Docker deployment strategy during later design phases. + +## Development +This repository includes configurations for [devcontainer](https://code.visualstudio.com/docs/devcontainers/containers) VSCode extension. This extension allows us to standup a containerized development environment. More information about the CARMA Streets Dev Container Setup can be found [here](.devcontainer/README.md). + +## Base Images +To make creating new CARMA Streets services easier and to make our CI/CD more efficient, we have introduced new CARMA Streets base images. These images can be used as a starting point and common build/runtime environment for CARMA Streets services. There are currently two CARMA Streets base images , for which documentation and Dockerfiles can be found [Streets Service Base](streets_service_base/README.md) and [Streets Service Base Lanelet Aware](streets_service_base_lanelet_aware/README.md). # CARMAStreets The primary carma-streets repository can be found [here](https://github.com/usdot-fhwa-stol/carma-streets) and is part of the [USDOT FHWA STOL](https://github.com/usdot-fhwa-stol/) github organization. Documentation on how the carma-streets functions, how it will evolve over time, and how you can contribute can be found at the above links, as well as on the [Doxygen Source Code Documentation](https://usdot-fhwa-stol.github.io/documentation/carma-streets/). +## Data Collection +**CARMA Streets** services have several source from which data about usecase performance can be pulled. **Kafka Message Topics** can be pulled using the `collect_kafka_logs.py` script and includes message traffic on a configurable list of topics. Service log files can be found in a given **CARMA Streets** service `logs/` directory. The `collect_service_logs.sh` adds all log files in this directory to a zip file and then deletes the originals. Some **CARMA Streets** services can configurable log performance in `.csv` file in the `logs/` directory. + +### Collect Kafka Logs +This script uses `docker exec` to ssh into a running kafka container. Then using kafka container scripts to read all kafka data from a list of provided topics. +``` +usage: collect_kafka_logs.py [-h] [--start_timestamp START_TIMESTAMP] [--end_timestamp END_TIMESTAMP] [--start_hours_ago START_HOURS_AGO] [--end_hours_ago END_HOURS_AGO] + [--topics TOPICS [TOPICS ...]] [--timeout TIMEOUT] [--zip ZIP] + outdir + +Script to grab data from kafka + +positional arguments: + outdir Folder name for the resulting folder logs are placed in + +options: + -h, --help show this help message and exit + --start_timestamp START_TIMESTAMP + Unix timestamp (seconds) for the first message to grab. Exclusive with start_hours_ago. + --end_timestamp END_TIMESTAMP + Unix timestamp (seconds) for the last message to grab. Exclusive with end_hours_ago. + --start_hours_ago START_HOURS_AGO + float hours before current time to grab first message. Exclusive with start_timestamp. + --end_hours_ago END_HOURS_AGO + float hours before current time to grab last message. Exclusive with start_timestamp. + --topics TOPICS [TOPICS ...] + list of topics to grab data from + --timeout TIMEOUT timeout for receiving messages on a topic, default is 5 seconds + --zip ZIP bool flag. When set to true, folder is compressed into a zip file. +``` +### Collection Service Logs +This script collects all **CARMA Streets** service log files, adds them to a zip file. +``` +usage: collect_service_logs.sh [-h | --help] [--output .zip] + [-c | --clear] + +options: +-h, --help Show usage +--output, -o Name of the resulting zip file container CARMA Streets Service logs +--clear, -c Adding this flag will delete log directories after creating zip. +``` ## Contribution Welcome to the CARMA contributing guide. Please read this guide to learn about our development process, how to propose pull requests and improvements, and how to build and test your changes to this project. [CARMA Contributing Guide](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) diff --git a/build.sh b/build.sh index 245b0cab5..ba3660b47 100755 --- a/build.sh +++ b/build.sh @@ -16,41 +16,51 @@ # script executes all kafka_clients and scheduling service build and coverage steps so that they can be singularly # wrapped by the sonarcloud build-wrapper set -e +# For lanelet aware streets services like message_services and intersection_model +source /opt/ros/melodic/setup.bash +source /opt/carma_lanelet2/setup.bash COVERAGE_FLAGS="-g --coverage -fprofile-arcs -ftest-coverage" # make install for these subdirectories MAKE_INSTALL_DIRS=( + "streets_utils/json_utils" + "streets_utils/streets_messages" "streets_utils/streets_service_configuration" "kafka_clients" "streets_utils/streets_service_base" "streets_utils/streets_vehicle_list" "streets_utils/streets_tsc_configuration" "streets_utils/streets_desired_phase_plan" + "streets_utils/streets_phase_control_schedule" + "streets_utils/streets_timing_plan" "streets_utils/streets_signal_phase_and_timing" "streets_utils/streets_api/intersection_client_api" "streets_utils/streets_vehicle_scheduler" "streets_utils/streets_api/intersection_server_api" "streets_utils/streets_signal_optimization" + "streets_utils/streets_snmp_cmd" ) # only make for these subdirectories MAKE_ONLY_DIRS=( "scheduling_service" - # "intersection_model" - # "message_services" + "intersection_model" + "message_services" "signal_opt_service" "tsc_client_service" + "sensor_data_sharing_service" ) for DIR in "${MAKE_INSTALL_DIRS[@]}" "${MAKE_ONLY_DIRS[@]}"; do - mkdir /home/carma-streets/"$DIR"/build - cd /home/carma-streets/"$DIR"/build - cmake -DCMAKE_CXX_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_C_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_BUILD_TYPE="Debug" .. - make -j + cd /home/carma-streets/"$DIR" + # Avoid catkin rebuilding gtest/gmock executables + # Build on libraries position independent + cmake -Bbuild -DCMAKE_CXX_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_C_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_BUILD_TYPE="Debug" -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DCATKIN_ENABLE_TESTING=OFF -DCMAKE_PREFIX_PATH="/opt/carma/cmake/;/opt/carma_lanelet2/" + cmake --build build for MAKE_INSTALL_DIR in "${MAKE_INSTALL_DIRS[@]}"; do if [ "$DIR" == "$MAKE_INSTALL_DIR" ]; then - make -j install + cmake --install build fi done done diff --git a/build_scripts/README.md b/build_scripts/README.md new file mode 100644 index 000000000..76c6af0ec --- /dev/null +++ b/build_scripts/README.md @@ -0,0 +1,7 @@ +# Build Scripts +## Introduction +The directory contains different scripts used for the build process of **CARMA Streets** service images and base images. +## Install Streets Service Base Dependencies +This script is used to install some common dependencies of **CARMA Streets** services. It assumes that build tools like a compiler, cmake, and other basic C++ build and test dependencies are already installed. Currently we use this script to install **CARMA Streets** service dependencies on top of `carma-builds-64x` (https://github.com/usdot-fhwa-stol/carma-builds) images to create base images for **CARMA Streets** services. +## Install Lanelet2 dependencies +This script installs [lanelet2](https://github.com/fzi-forschungszentrum-informatik/Lanelet2) along with its depedencies. **CARMA Streets** services that require spatial understanding of the surrounding roadway use [lanelet2](https://github.com/fzi-forschungszentrum-informatik/Lanelet2) maps and libraries. This script is used in conjuction with the [streets_service_base_lanelet_aware](../streets_service_base_lanelet_aware/README.md) Dockerfile to create a base image which includes these dependencies. \ No newline at end of file diff --git a/build_scripts/install_dependencies.sh b/build_scripts/install_dependencies.sh index 05f8930d3..c8ebdd98d 100755 --- a/build_scripts/install_dependencies.sh +++ b/build_scripts/install_dependencies.sh @@ -3,8 +3,10 @@ # exit on errors set -e +# Get ubuntu distribution code name. All STOL APT debian packages are pushed to S3 bucket based on distribution codename. +. /etc/lsb-release # add the STOL APT repository -echo "deb [trusted=yes] http://s3.amazonaws.com/stol-apt-repository develop main" > /etc/apt/sources.list.d/stol-apt-repository.list +echo "deb [trusted=yes] http://s3.amazonaws.com/stol-apt-repository ${DISTRIB_CODENAME} main" > /etc/apt/sources.list.d/stol-apt-repository.list apt-get update diff --git a/build_scripts/install_lanelet2_dependencies.sh b/build_scripts/install_lanelet2_dependencies.sh new file mode 100755 index 000000000..5c0a62ca6 --- /dev/null +++ b/build_scripts/install_lanelet2_dependencies.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# Script assumes base image of carma-builds-64x +set -e +# Get ubuntu distribution code name. All STOL APT debian packages are pushed to S3 bucket based on distribution codename. +# shellcheck source=/dev/null +source /etc/lsb-release +# add the STOL APT repository +echo "deb [trusted=yes] http://s3.amazonaws.com/stol-apt-repository ${DISTRIB_CODENAME} main" > /etc/apt/sources.list.d/stol-apt-repository.list +echo "deb http://packages.ros.org/ros/ubuntu ${DISTRIB_CODENAME} main" > /etc/apt/sources.list.d/ros-latest.list +DEBIAN_FRONTEND=noninteractive apt install --no-install-recommends --yes --quiet gnupg +wget -qO- https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - +apt-get update +DEPENDENCIES=( + autotools-dev + automake + sqlite3 + libsqlite3-dev + libpugixml-dev + libgeographic-dev + ros-melodic-catkin + python-rospkg + libeigen3-dev + libtool + libboost-all-dev +) +DEBIAN_FRONTEND=noninteractive apt install --no-install-recommends --yes --quiet "${DEPENDENCIES[@]}" +# Install PROJ, a package for coordinate transformations +git clone https://github.com/OSGeo/PROJ.git /tmp/PROJ --branch 6.2.1 +cd /tmp/PROJ +./autogen.sh +./configure +make +make install +cd .. +rm -r PROJ + +# Download a cmake module for PROJ +wget -P /usr/local/share/cmake-3.27/Modules https://raw.githubusercontent.com/mloskot/cmake-modules/master/modules/FindPROJ4.cmake + +cd /tmp +# Once catkin is installed only the required lanelet2 packages will be pulled in from carma +# NOTE: The lanelet2_python package requires additional dependencies that have not yet been installed so it is removed for now +mkdir carma_lanelet2 +cd carma_lanelet2 +mkdir src +cd src +git init +echo "temp" +git remote add origin -f https://github.com/usdot-fhwa-stol/autoware.ai.git +git config core.sparsecheckout true +# See https://www.shellcheck.net/wiki/SC2129 for reference on syntax +{ +echo "common/hardcoded_params/*" +echo "common/lanelet2_extension/*" +echo "lanelet2/*" +echo "mrt_cmake_modules/*" + } >> .git/info/sparse-checkout +git pull --depth 1 origin refactor_lanelet2_extension +git checkout refactor_lanelet2_extension +rm -r lanelet2/lanelet2_python +rm -r lanelet2/lanelet2_examples +cd /tmp/carma_lanelet2/ +source /opt/ros/melodic/setup.bash +ROS_VERSION=1 LANELET2_EXTENSION_LOGGER_TYPE=1 catkin_make install -DCMAKE_INSTALL_PREFIX=/opt/carma_lanelet2 -DCATKIN_DEVEL_PREFIX=/tmp/carma_lanelet2/src +rm -r /tmp/carma_lanelet2 + + diff --git a/build_scripts/install_rest_server_dependencies.sh b/build_scripts/install_rest_server_dependencies.sh index 1d59e710f..002639bf8 100755 --- a/build_scripts/install_rest_server_dependencies.sh +++ b/build_scripts/install_rest_server_dependencies.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash # exit on errors set -e @@ -7,12 +7,14 @@ set -e apt-get update # NOTE: libwebsockets-dev from Ubuntu 20 on is sufficient -DEPENDENCIES="libssl-dev \ - qtbase5-dev \ - qtbase5-dev-tools" +DEPENDENCIES=( + libssl-dev + qtbase5-dev + qtbase5-dev-tools +) # install all things needed for deployment, always done -apt-get install -y $DEPENDENCIES +apt-get install -y "${DEPENDENCIES[@]}" cd /home/carma-streets/ext git clone https://github.com/etherealjoy/qhttpengine.git diff --git a/build_scripts/install_streets_service_base_dependencies.sh b/build_scripts/install_streets_service_base_dependencies.sh new file mode 100755 index 000000000..baea02b32 --- /dev/null +++ b/build_scripts/install_streets_service_base_dependencies.sh @@ -0,0 +1,49 @@ +#!/bin/bash +set -e +# Get ubuntu distribution code name. All STOL APT debian packages are pushed to S3 bucket based on distribution codename. +# shellcheck source=/dev/null +source /etc/lsb-release +# add the STOL APT repository +echo "deb [trusted=yes] http://s3.amazonaws.com/stol-apt-repository ${DISTRIB_CODENAME} main" > /etc/apt/sources.list.d/stol-apt-repository.list +apt update +DEPENDENCIES=( + libboost-all-dev + carma-clock-1 +) + +DEBIAN_FRONTEND=noninteractive apt install --no-install-recommends --yes --quiet "${DEPENDENCIES[@]}" + +# Install librdkafka from instructions provided here https://github.com/confluentinc/librdkafka/tree/master/packaging/cmake +echo " ------> Install librdkafka..." +cd /tmp +git clone https://github.com/confluentinc/librdkafka.git -b v2.2.0 +cd librdkafka/ +cmake -H. -B_cmake_build +cmake --build _cmake_build +cmake --build _cmake_build --target install +cd ../ +rm -r librdkafka + +# Install rapidjson commit instead of release since there has been no new release since August 2016 +# but repo is still under active development +echo " ------> Install rapidjson..." +cd /tmp +git clone https://github.com/Tencent/rapidjson +cd rapidjson/ +git checkout a95e013b97ca6523f32da23f5095fcc9dd6067e5 +cmake -Bbuild -DCMAKE_POSITION_INDEPENDENT_CODE=ON +cmake --build build +cmake --install build +cd .. +rm -r rapidjson + +# Install spdlog +echo " ------> Install spdlog... " +cd /tmp +git clone https://github.com/gabime/spdlog.git -b v1.12.0 +cd spdlog +cmake -Bbuild -DCMAKE_POSITION_INDEPENDENT_CODE=ON +cmake --build build +cmake --install build +cd .. +rm -r spdlog diff --git a/build_scripts/install_test_dependencies.sh b/build_scripts/install_test_dependencies.sh index 20e9001f7..b56098ecd 100755 --- a/build_scripts/install_test_dependencies.sh +++ b/build_scripts/install_test_dependencies.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash # exit on errors set -e @@ -7,10 +7,14 @@ set -e apt-get update -# NOTE: libwebsockets-dev from Ubuntu 20 on is sufficient -DEPENDENCIES="python3-pip " +# NOTE: lxml python library dependes on libxml2 and libxslt1-dev. lxml is a dependency of gcovr + +DEPENDENCIES=( + python3-pip + python3-lxml +) # install all things needed for deployment, always done -apt-get install -y $DEPENDENCIES -pip3 install gcovr +DEBIAN_FRONTEND=noninteractive apt-get install --yes --quiet --no-install-recommends "${DEPENDENCIES[@]}" +python3 -m pip install gcovr diff --git a/collect_kafka_logs.py b/collect_kafka_logs.py index e822fcdb5..d4df815a1 100644 --- a/collect_kafka_logs.py +++ b/collect_kafka_logs.py @@ -1,9 +1,25 @@ +#!/usr/bin/python3 +# Copyright (C) 2024 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. import os import time import re import signal import subprocess import argparse +from pathlib import Path +import shutil ## Python script to download and store kafka logs @@ -71,37 +87,39 @@ def store_kafka_topic(topic, dir, timeout, start_time, end_time): def main(): # Default list of topics, aka all topics from https://usdot-carma.atlassian.net/wiki/spaces/CRMSRT/pages/2317549999/CARMA-Streets+Messaging - # topic desire_phase_plan is in the list from confluence, but doesn't exist in kafka as of 2022/12/05 topics = ['v2xhub_scheduling_plan_sub' ,'v2xhub_bsm_in', 'v2xhub_mobility_operation_in', 'v2xhub_mobility_path_in', - 'vehicle_status_intent_output', 'v2xhub_map_msg_in', 'modified_spat', 'tsc_config_state', 'desired_phase_plan'] + 'vehicle_status_intent_output', 'v2xhub_map_msg_in', 'modified_spat', 'tsc_config_state', 'desired_phase_plan', + 'v2xhub_sdsm_sub', 'v2xhub_sim_sensor_detected_object', 'v2xhub_sdsm_tra', 'desire_phase_plan', 'time_sync'] timeout = 5 # Get arguments # The idea here was to give the user the bare minimum options, and make the default condition the most used. parser = argparse.ArgumentParser(description='Script to grab data from kafka') - parser.add_argument('outfile', help='Filename for the resulting zip file', type=str) # Required argument + parser.add_argument('outdir', help='Folder name for the resulting folder logs are placed in', type=str) # Required argument start_group = parser.add_mutually_exclusive_group() end_group = parser.add_mutually_exclusive_group() start_group.add_argument('--start_timestamp', help='Unix timestamp (seconds) for the first message to grab. Exclusive with start_hours_ago. ', type=int) end_group.add_argument('--end_timestamp', help='Unix timestamp (seconds) for the last message to grab. Exclusive with end_hours_ago. ', type=int) start_group.add_argument('--start_hours_ago', help='float hours before current time to grab first message. Exclusive with start_timestamp. ', type=float) end_group.add_argument('--end_hours_ago', help='float hours before current time to grab last message. Exclusive with start_timestamp. ', type=float) - parser.add_argument('--topics', type=str, nargs='+', help=f'list of topics to grab data from') - parser.add_argument('--timeout', type=float, help=f'timeout for receiving messages on a topic, default is 5 seconds') + parser.add_argument('--topics', type=str, nargs='+', help='list of topics to grab data from') + parser.add_argument('--timeout', type=float, help='timeout for receiving messages on a topic, default is 5 seconds') + parser.add_argument('--zip', type=bool,help='bool flag. When set to true, folder is compressed into a zip file.', default=False) args = parser.parse_args() # Correct and validate outfile name - if args.outfile[-4:] == '.zip': - outfile = args.outfile[:-4] + if args.outdir[-4:] == '.zip': + outdir = args.outdir[:-4] else: - outfile = args.outfile - - if os.path.isdir(f'{os.getcwd()}/{outfile}'): - print(f'folder {outfile} exists, please remove or rename') + outdir = args.outdir + # Check if output directory already exists + if Path(Path.cwd()/outdir).is_dir(): + print(f'folder {outdir} exists, please remove or rename') return - elif os.path.isfile(f'{os.getcwd()}/{outfile}.zip'): - print(f'zip file {outfile}.zip exists, please remove or rename') + # Check if zip file already exists + elif Path(Path.cwd()/f'{outdir}.zip').is_file(): + print(f'zip file {outdir}.zip exists, please remove or rename') return # Convert given time to unix timestamp (in ms) @@ -123,17 +141,17 @@ def main(): topics = args.topics if args.timeout: timeout = args.timeout - - os.system(f'mkdir {outfile}') + Path(outdir).mkdir(exist_ok=False) for topic in topics: - ret = store_kafka_topic(topic, outfile, timeout, start_time, end_time) + ret = store_kafka_topic(topic, outdir, timeout, start_time, end_time) if ret != 0: - print('received error, stopping execution') - return + print(f'received error on topic {topic}, going to next topic') - print('Available logs collected, zipping and removing the temporary folder') - os.system(f'zip -r {outfile}.zip {outfile}') - os.system(f'rm -r {outfile}') + print('Available logs collected') + if args.zip: + print('Zipping and removing the temporary folder') + shutil.make_archive(base_name=outdir, format='zip', root_dir=outdir) + shutil.rmtree(path=outdir) if __name__ == "__main__": diff --git a/collect_service_logs.sh b/collect_service_logs.sh index 2ff76338a..70752fc98 100755 --- a/collect_service_logs.sh +++ b/collect_service_logs.sh @@ -1,11 +1,116 @@ +#!/bin/bash +# Copyright (C) 2024 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations under # the License. - -# script collects all service logs files and zips them into an archive and then removes the source files. +####################################### +# Prints the script's argument descriptions +# Globals: +# None +# Arguments: +# None +# Returns: +# 0 +####################################### set -e -zip -r service_logs.zip ./scheduling_service/logs ./intersection_model/logs ./tsc_client_service/logs ./message_services/logs ./signal_opt_service/logs -sudo rm ./scheduling_service/logs/* ./intersection_model/logs/* ./tsc_client_service/logs/* ./message_services/logs/* ./signal_opt_service/logs/* +function print_help() { + command cat <<-HELP +options: +-h, --help Show usage +--output, -o Name of the resulting zip file container CARMA Streets Service logs +--clear, -c Adding this flag will delete log directories after creating zip. + +HELP +} +####################################### +# Prints a string to standard error +# Globals: +# None +# Arguments: +# String to print +# Returns: +# 0 +####################################### +function err() { + echo "$*" >&2 +} +####################################### +# Prints the script's usage +# Globals: +# None +# Arguments: +# None +# Returns: +# 0 +####################################### +function print_usage() { + command cat <<-USAGE +usage: collect_service_logs.sh [-h | --help] [--output .zip] + [-c | --clear] +USAGE +} +####################################### +# Collects all service logs files and zips them into an archive and then configurable removes the source files. +# Globals: +# None +# Arguments: +# None +# Returns: +# 0 +####################################### +function main() { + while :; do + case "$1" in + -h|--help) + print_usage + echo "" + print_help + exit 0 + ;; + -c|--clear) + echo "Clearing log directories after zip file is created!" + clear=true + ;; + --output|-o) + shift + if [ "$#" -eq 0 ]; then + err "error: option 'output' requires a value" + print_usage + exit 129 + fi + output="$1" + ;; + *) + if [[ "$1" == -* ]]; then + err "unknown option '$1'" + print_usage + exit 129 + fi + break + esac + shift + done + if [ "$output" ]; + then + echo "Writing zip file ${output}.zip" + zip -rq ./"${output}".zip ./scheduling_service/logs ./intersection_model/logs ./tsc_client_service/logs ./message_services/logs ./signal_opt_service/logs ./sensor_data_sharing_service/logs + else + filename=$(date "+%FT%H%M%S") + echo "Writing zip file ${filename}.zip" + zip -rq ./"${filename}".zip ./scheduling_service/logs ./intersection_model/logs ./tsc_client_service/logs ./message_services/logs ./signal_opt_service/logs ./sensor_data_sharing_service/logs + fi + if [ "$clear" = true ]; + then + rm ./scheduling_service/logs/* ./intersection_model/logs/* ./tsc_client_service/logs/* ./message_services/logs/* ./signal_opt_service/logs/* ./sensor_data_sharing_service/logs/* + fi +} +main "$@" \ No newline at end of file diff --git a/coverage.sh b/coverage.sh index 534b2bab5..f15b5c31d 100755 --- a/coverage.sh +++ b/coverage.sh @@ -15,10 +15,28 @@ # script to run tests, generate test-coverage, and store coverage reports in a place # easily accessible to sonar. Test names should follow convention runTests +set -e +# For lanelet aware streets services like message_services and intersection_model +source /opt/ros/melodic/setup.bash +source /opt/carma_lanelet2/setup.bash cd /home/carma-streets mkdir test_results +cd /home/carma-streets/streets_utils/json_utils/build/ +./json_utils_test --gtest_output=xml:../../../test_results/ +cd /home/carma-streets/streets_utils/json_utils +mkdir coverage +cd /home/carma-streets/ +gcovr --sonarqube streets_utils/json_utils/coverage/coverage.xml -s -f streets_utils/json_utils/ -r . + +cd /home/carma-streets/streets_utils/streets_messages/build/ +./streets_messages_lib_test --gtest_output=xml:../../../test_results/ +cd /home/carma-streets/streets_utils/streets_messages +mkdir coverage +cd /home/carma-streets/ +gcovr --sonarqube streets_utils/streets_messages/coverage/coverage.xml -s -f streets_utils/streets_messages/ + cd /home/carma-streets/streets_utils/streets_service_configuration/build/ ./streets_service_configuration_test --gtest_output=xml:../../../test_results/ cd /home/carma-streets/streets_utils/streets_service_configuration @@ -83,6 +101,20 @@ mkdir coverage cd /home/carma-streets/ gcovr --sonarqube streets_utils/streets_desired_phase_plan/coverage/coverage.xml -s -f streets_utils/streets_desired_phase_plan/ -r . +cd /home/carma-streets/streets_utils/streets_phase_control_schedule/build/ +./streets_phase_control_schedule_test --gtest_output=xml:../../../test_results/ +cd /home/carma-streets/streets_utils/streets_phase_control_schedule +mkdir coverage +cd /home/carma-streets/ +gcovr --sonarqube streets_utils/streets_phase_control_schedule/coverage/coverage.xml -s -f streets_utils/streets_phase_control_schedule/ -r . + +cd /home/carma-streets/streets_utils/streets_timing_plan/build/ +./streets_timing_plan_test --gtest_output=xml:../../../test_results/ +cd /home/carma-streets/streets_utils/streets_timing_plan +mkdir coverage +cd /home/carma-streets/ +gcovr --sonarqube streets_utils/streets_timing_plan/coverage/coverage.xml -s -f streets_utils/streets_timing_plan/ -r . + cd /home/carma-streets/streets_utils/streets_signal_optimization/build/ ./streets_signal_optimization_test --gtest_output=xml:../../../test_results/ cd /home/carma-streets/streets_utils/streets_signal_optimization @@ -90,21 +122,28 @@ mkdir coverage cd /home/carma-streets/ gcovr --sonarqube streets_utils/streets_signal_optimization/coverage/coverage.xml -s -f streets_utils/streets_signal_optimization/ -r . -# cd /home/carma-streets/message_services/build/ -# # Currently only running a subset of message_services tests. TODO: Fix the remaining test cases. -# ./message_services_test --gtest_output=xml:../../test_results/ -# cd /home/carma-streets/message_services/ -# mkdir coverage -# cd /home/carma-streets/ -# gcovr --sonarqube message_services/coverage/coverage.xml -s -f message_services/ -r . +cd /home/carma-streets/streets_utils/streets_snmp_cmd/build/ +./streets_snmp_cmd_test --gtest_output=xml:../../../test_results/ +cd /home/carma-streets/streets_utils/streets_snmp_cmd +mkdir coverage +cd /home/carma-streets/ +gcovr --sonarqube streets_utils/streets_snmp_cmd/coverage/coverage.xml -s -f streets_utils/streets_snmp_cmd/ -r . + +cd /home/carma-streets/message_services/build/ +# Currently only running a subset of message_services tests. TODO: Fix the remaining test cases. +./message_services_test --gtest_output=xml:../../test_results/ +cd /home/carma-streets/message_services/ +mkdir coverage +cd /home/carma-streets/ +gcovr --sonarqube message_services/coverage/coverage.xml -s -f message_services/ -r . -# cd /home/carma-streets/intersection_model/build/ -# ./intersection_model_test ---gtest_output=xml:../../test_results/ -# cd /home/carma-streets/intersection_model/ -# mkdir coverage -# cd /home/carma-streets/ -# gcovr --exclude=intersection_model/src/server/ --exclude=intersection_model/test/ --exclude=intersection_model/build/src/ --sonarqube intersection_model/coverage/coverage.xml -s -f intersection_model/ -r . +cd /home/carma-streets/intersection_model/build/ +./intersection_model_test ---gtest_output=xml:../../test_results/ +cd /home/carma-streets/intersection_model/ +mkdir coverage +cd /home/carma-streets/ +gcovr --exclude=intersection_model/src/server/ --exclude=intersection_model/test/ --exclude=intersection_model/build/src/ --sonarqube intersection_model/coverage/coverage.xml -s -f intersection_model/ -r . cd /home/carma-streets/signal_opt_service/build/ @@ -121,3 +160,9 @@ mkdir coverage cd /home/carma-streets/ gcovr --sonarqube tsc_client_service/coverage/coverage.xml -s -f tsc_client_service/ -r . +cd /home/carma-streets/sensor_data_sharing_service/build/ +./sensor_data_sharing_service_test --gtest_output=xml:../../test_results/ +cd /home/carma-streets/sensor_data_sharing_service/ +mkdir coverage +cd /home/carma-streets/ +gcovr --sonarqube sensor_data_sharing_service/coverage/coverage.xml -s -f sensor_data_sharing_service/ -r . diff --git a/docker-compose.yml b/docker-compose.yml index 03274f17f..9e8f44878 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -16,10 +16,31 @@ services: - zookeeper ports: - "9092:9092" + # Health check to confirm kafka server is healthy (script is a client) and all topics + # have been created (time_sync is last topic). + healthcheck: + test: ["CMD", "kafka-topics.sh", "--describe", "--bootstrap-server", "127.0.0.1:9092", "--topic", "time_sync"] + interval: 2s + timeout: 10s + retries: 10 environment: DOCKER_HOST_IP: ${DOCKER_HOST_IP} KAFKA_ADVERTISED_HOST_NAME: ${DOCKER_HOST_IP} - KAFKA_CREATE_TOPICS: "v2xhub_scheduling_plan_sub:1:1,v2xhub_bsm_in:1:1,v2xhub_mobility_operation_in:1:1,v2xhub_mobility_path_in:1:1,vehicle_status_intent_output:1:1,v2xhub_map_msg_in:1:1,modified_spat:1:1,desired_phase_plan:1:1, tsc_config_state:1:1" + KAFKA_CREATE_TOPICS: "\ + v2xhub_scheduling_plan_sub:1:1,\ + v2xhub_bsm_in:1:1,\ + v2xhub_mobility_operation_in:1:1,\ + v2xhub_mobility_path_in:1:1,\ + vehicle_status_intent_output:1:1,\ + v2xhub_map_msg_in:1:1,\ + modified_spat:1:1,\ + desired_phase_plan:1:1,\ + tsc_config_state:1:1,\ + v2xhub_sim_sensor_detected_object:1:1,\ + v2xhub_sdsm_sub:1:1,\ + desire_phase_plan:1:1,\ + v2xhub_sdsm_tra:1:1,\ + time_sync:1:1" KAFKA_ZOOKEEPER_CONNECT: zookeeper:2181 KAFKA_LOG_DIRS: "/kafka/kafka-logs" volumes: @@ -38,7 +59,8 @@ services: restart: on-failure network_mode: host depends_on: - - kafka + kafka: + condition: service_healthy environment: CONFIG_FILEPATH: /etc/kowl/kowl.yaml volumes: @@ -60,7 +82,7 @@ services: - ./mysql/localhost.sql:/docker-entrypoint-initdb.d/localhost.sql - mysql-datavolume:/var/lib/mysql php: - image: usdotfhwaops/php:7.5.1 + image: usdotfhwaops/php:7.6.0 container_name: php network_mode: host depends_on: @@ -69,7 +91,7 @@ services: tty: true v2xhub: - image: usdotfhwaops/v2xhubamd:7.5.1 + image: usdotfhwaops/v2xhubamd:7.6.0 container_name: v2xhub network_mode: host restart: always @@ -85,7 +107,7 @@ services: - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 - V2X_PORT=8686 - - INFRASTRUCTURE_ID=carma-streets_1 + - INFRASTRUCTURE_ID=rsu_123 - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 secrets: - mysql_password @@ -95,7 +117,7 @@ services: - /etc/localtime:/etc/localtime:ro - /etc/timezone:/etc/timezone:ro scheduling_service: - image: usdotfhwastolcandidate/scheduling_service:carma-system-4.4.3 + image: usdotfhwastol/scheduling_service:carma-system-4.5.0 command: sh -c "/wait && /home/carma-streets/scheduling_service/build/scheduling_service" build: context: . @@ -103,10 +125,13 @@ services: container_name: scheduling_service restart: always network_mode: host - depends_on: - - kafka - - intersection_model - - message_services + depends_on: + kafka: + condition: service_healthy + intersection_model: + condition: service_started + message_services: + condition: service_started environment: DOCKER_HOST_IP: ${DOCKER_HOST_IP} WAIT_HOSTS: localhost:8080 @@ -120,15 +145,16 @@ services: - /etc/timezone:/etc/timezone:ro message_services: - image: usdotfhwastolcandidate/message_services:carma-system-4.4.3 + image: usdotfhwastol/message_services:carma-system-4.5.0 build: context: . dockerfile: message_services/Dockerfile container_name: message_services restart: always network_mode: host - depends_on: - - kafka + depends_on: + kafka: + condition: service_healthy environment: DOCKER_HOST_IP: ${DOCKER_HOST_IP} volumes: @@ -138,7 +164,7 @@ services: - /etc/localtime:/etc/localtime:ro - /etc/timezone:/etc/timezone:ro intersection_model: - image: usdotfhwastolcandidate/intersection_model:carma-system-4.4.3 + image: usdotfhwastol/intersection_model:carma-system-4.5.0 build: context: . dockerfile: intersection_model/Dockerfile @@ -146,7 +172,8 @@ services: restart: always network_mode: host depends_on: - - kafka + kafka: + condition: service_healthy environment: DOCKER_HOST_IP: ${DOCKER_HOST_IP} volumes: @@ -156,7 +183,7 @@ services: - /etc/localtime:/etc/localtime:ro - /etc/timezone:/etc/timezone:ro signal_opt_service: - image: usdotfhwastolcandidate/signal_opt_service:carma-system-4.4.3 + image: usdotfhwastol/signal_opt_service:carma-system-4.5.0 command: sh -c "/wait && /home/carma-streets/signal_opt_service/build/signal_opt_service" build: context: . @@ -165,11 +192,16 @@ services: restart: always network_mode: host depends_on: - - kafka - - intersection_model - - message_services - - scheduling_service - - tsc_service + kafka: + condition: service_healthy + intersection_model: + condition: service_started + message_services: + condition: service_started + scheduling_service: + condition: service_started + tsc_service: + condition: service_started environment: DOCKER_HOST_IP: ${DOCKER_HOST_IP} WAIT_HOSTS: localhost:8080 @@ -182,7 +214,7 @@ services: - /etc/localtime:/etc/localtime:ro - /etc/timezone:/etc/timezone:ro tsc_service: - image: usdotfhwastolcandidate/tsc_service:carma-system-4.4.3 + image: usdotfhwastol/tsc_service:carma-system-4.5.0 command: sh -c "/wait && /home/carma-streets/tsc_client_service/build/traffic_signal_controller_service" build: context: . @@ -191,22 +223,52 @@ services: restart: always network_mode: host depends_on: - - kafka - - intersection_model + kafka: + condition: service_healthy + intersection_model: + condition: service_started environment: DOCKER_HOST_IP: ${DOCKER_HOST_IP} WAIT_HOSTS: localhost:8080 WAIT_HOSTS_TIMEOUT: 300 WAIT_SLEEP_INTERVAL: 30 WAIT_HOST_CONNECT_TIMEOUT: 30 - SIMULATION_MODE: TRUE + SIMULATION_MODE: FALSE CONFIG_FILE_PATH: ../manifest.json TIME_SYNC_TOPIC: time_sync + LOGS_DIRECTORY: /home/carma-streets/tsc_client_service/logs/ volumes: - ./tsc_client_service/manifest.json:/home/carma-streets/tsc_client_service/manifest.json - ./tsc_client_service/logs/:/home/carma-streets/tsc_client_service/logs/ - /etc/localtime:/etc/localtime:ro - /etc/timezone:/etc/timezone:ro + sensor_data_sharing_service: + image: usdotfhwastol/sensor_data_sharing_service:carma-system-4.5.0 + build: + context: . + dockerfile: sensor_data_sharing_service/Dockerfile + container_name: sensor_data_sharing_service + restart: always + network_mode: host + depends_on: + kafka: + condition: service_healthy + environment: + SIMULATION_MODE: FALSE + CONFIG_FILE_PATH: /home/carma-streets/sensor_data_sharing_service/manifest.json + TIME_SYNC_TOPIC: time_sync + LOGS_DIRECTORY: /home/carma-streets/sensor_data_sharing_service/logs/ + LANELET2_MAP: /home/carma-streets/MAP/Intersection.osm + INFRASTRUCTURE_ID: "rsu_123" + SENSOR_JSON_FILE_PATH: /home/carma-streets/sensor_configurations/sensors.json + + volumes: + - ./sensor_data_sharing_service/manifest.json:/home/carma-streets/sensor_data_sharing_service/manifest.json + - ./sensor_data_sharing_service/logs/:/home/carma-streets/sensor_data_sharing_service/logs/ + - ./MAP/:/home/carma-streets/intersection_model/MAP/ + - ./sensor_configurations/:/home/carma-streets/sensor_configurations/ + - /etc/localtime:/etc/localtime:ro + - /etc/timezone:/etc/timezone:ro secrets: mysql_password: file: ./secrets/mysql_password.txt diff --git a/intersection_model/CMakeLists.txt b/intersection_model/CMakeLists.txt index 3b19364cd..befd6a876 100644 --- a/intersection_model/CMakeLists.txt +++ b/intersection_model/CMakeLists.txt @@ -1,10 +1,6 @@ cmake_minimum_required(VERSION 3.10.2) project(intersection_model) -link_directories( - "/usr/lib" - "/usr/local/lib" - ${catkin_LIBRARY_DIRS} - ${catkin_INCLUDE_DIRS}) + find_package(Boost COMPONENTS thread filesystem system REQUIRED) find_package(spdlog REQUIRED) add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) diff --git a/intersection_model/Dockerfile b/intersection_model/Dockerfile index 67704e9ac..493d164ea 100644 --- a/intersection_model/Dockerfile +++ b/intersection_model/Dockerfile @@ -13,16 +13,18 @@ RUN echo " ------> Install googletest..." WORKDIR /home/carma-streets/ RUN mkdir -p /home/carma-streets/ext WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/google/googletest/ +RUN git clone https://github.com/google/googletest.git -b v1.13.0 WORKDIR /home/carma-streets/ext/googletest/ -RUN cmake . +RUN mkdir build +WORKDIR /home/carma-streets/ext/googletest/build +RUN cmake .. RUN make RUN make install # Install spdlog RUN echo " ------> Install spdlog... " WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/gabime/spdlog.git +RUN git clone https://github.com/gabime/spdlog.git -b v1.12.0 WORKDIR /home/carma-streets/ext/spdlog/ RUN mkdir build WORKDIR /home/carma-streets/ext/spdlog/build @@ -32,7 +34,7 @@ RUN make install # Install librdkafka RUN echo " ------> Install librdkafka..." WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/confluentinc/librdkafka.git +RUN git clone https://github.com/confluentinc/librdkafka.git -b v2.2.0 WORKDIR /home/carma-streets/ext/librdkafka/ RUN cmake -H. -B_cmake_build RUN cmake --build _cmake_build @@ -43,6 +45,7 @@ RUN echo " ------> Install rapidjson..." WORKDIR /home/carma-streets/ext RUN git clone https://github.com/Tencent/rapidjson WORKDIR /home/carma-streets/ext/rapidjson/ +RUN git checkout a95e013b97ca6523f32da23f5095fcc9dd6067e5 RUN mkdir build WORKDIR /home/carma-streets/ext/rapidjson/build RUN cmake .. && make -j diff --git a/intersection_model/src/intersection_model.cpp b/intersection_model/src/intersection_model.cpp index 24141af2c..4e45eea0b 100755 --- a/intersection_model/src/intersection_model.cpp +++ b/intersection_model/src/intersection_model.cpp @@ -81,12 +81,9 @@ namespace intersection_model lanelet::traffic_rules::TrafficRulesFactory::create(lanelet::Locations::Germany, lanelet::Participants::Vehicle, lanelet::traffic_rules::TrafficRules::Configuration())}; - lanelet::routing::RoutingCostPtrs costPtrs{ - std::make_shared(this->laneChangeCost, this->minLaneChangeLength), - std::make_shared(this->laneChangeCost)}; - lanelet::routing::RoutingGraph::Configuration configuration; - configuration.insert(std::make_pair(lanelet::routing::RoutingGraph::ParticipantHeight, this->participantHeight)); - this->vehicleGraph_ptr = lanelet::routing::RoutingGraph::build(*this->map, *trafficRules, costPtrs, configuration); + + this->vehicleGraph_ptr = lanelet::routing::RoutingGraph::build(*this->map, *trafficRules); + if ( !this->vehicleGraph_ptr ) { return false; @@ -646,4 +643,4 @@ namespace intersection_model int_info.entering_lanelets_info.clear(); int_info.departure_lanelets_info.clear(); } -} \ No newline at end of file +} diff --git a/kafka_clients/README.md b/kafka_clients/README.md new file mode 100644 index 000000000..557314f45 --- /dev/null +++ b/kafka_clients/README.md @@ -0,0 +1,17 @@ +# Kafka Clients +## Introduction +[librdkafka_github]: https://github.com/confluentinc/librdkafka +This is a library that wraps [librdkafka][librdkafka_github] C++ kafka client library. It consists of three main classes: +`kafka_client`: Class used to setup Kafka connection and create producers and consumers. +`kafka_producer_worker`: Kafka producer. Constructor takes in kafka broker, topic to produce to, and an optional parameter for partition. +`kafka_consumer_worker`: Kafka consumer. Constructor takes in kafka broker, topic to subscribe to, and consumer group to join. +More [Kafka Documentation](https://kafka.apache.org/documentation/) is avaiable online. +## Usage +To include in you CMake project simply find the `kafka_clients_lib` package and link it to your target. Note: this package requires that [librdkafka][librdkafka_github] is installed. It also requires that [spdlog](https://github.com/gabime/spdlog) is installed. +```cmake +find_package(kafka_clients_lib REQUIRED) +target_link_libraries( + PUBLIC + kafka_clients_lib::kafka_clients_lib +) +``` \ No newline at end of file diff --git a/kafka_clients/include/kafka_client.h b/kafka_clients/include/kafka_client.h index 344284f3c..4e333df62 100644 --- a/kafka_clients/include/kafka_client.h +++ b/kafka_clients/include/kafka_client.h @@ -15,11 +15,10 @@ namespace kafka_clients class kafka_client { public: - std::shared_ptr create_consumer(const std::string &broker_str, const std::string &topic_str, + virtual ~kafka_client()=default; + virtual std::shared_ptr create_consumer(const std::string &broker_str, const std::string &topic_str, const std::string &group_id_str) const; - std::shared_ptr create_producer(const std::string &broker_str, const std::string &topic_str) const; - rapidjson::Document read_json_file(const std::string &json_file) const; - std::string get_value_by_doc(rapidjson::Document &doc, const char *key) const; + virtual std::shared_ptr create_producer(const std::string &broker_str, const std::string &topic_str) const; }; } diff --git a/kafka_clients/include/mock_kafka_client.h b/kafka_clients/include/mock_kafka_client.h new file mode 100644 index 000000000..9f93815a9 --- /dev/null +++ b/kafka_clients/include/mock_kafka_client.h @@ -0,0 +1,19 @@ +#pragma once +#include "kafka_client.h" +#include +namespace kafka_clients { + /** + * @brief Mock kafka client used for unit testing using gmock. For documentation using gmock mocks + * (https://google.github.io/googletest/gmock_for_dummies.html). + * + * @author + */ + class mock_kafka_client : public kafka_client { + public: + mock_kafka_client() {}; + ~mock_kafka_client() = default; + MOCK_METHOD(std::shared_ptr, create_consumer,(const std::string &broker_str, const std::string &topic_str, const std::string &group_id_str),(const, override)); + MOCK_METHOD(std::shared_ptr, create_producer,(const std::string &broker_str, const std::string &topic_str),(const, override)); + + }; +} \ No newline at end of file diff --git a/kafka_clients/readme.md b/kafka_clients/readme.md deleted file mode 100644 index a3f544651..000000000 --- a/kafka_clients/readme.md +++ /dev/null @@ -1,29 +0,0 @@ - -``` -git clone https://github.com/google/googletest/ -cmake . -make -sudo make install - -refer to https://github.com/edenhill/librdkafka -./configure - make - sudo make install - -git clone https://github.com/gabime/spdlog.git -cd spdlog && mkdir build && cd build -cmake .. && make -j -sudo make install - -https://github.com/Tencent/rapidjson -git clone https://github.com/Tencent/rapidjson -cd rapidjson && mkdir build && cd build -cmake .. -make -sudo make install - -git clone https://github.com/usdot-fhwa-stol/carma-streets -cd kafka_clients && mkdir build && cd build -cmake .. && make -j -sudo make install -``` \ No newline at end of file diff --git a/message_services/CMakeLists.txt b/message_services/CMakeLists.txt index 28f54bcc5..01f453cf3 100644 --- a/message_services/CMakeLists.txt +++ b/message_services/CMakeLists.txt @@ -8,7 +8,6 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") -link_directories(${catkin_LIBRARY_DIRS} ${catkin_INCLUDE_DIRS}) find_package(PROJ4) find_package(Eigen3 3.3 NO_MODULE) find_package(catkin COMPONENTS diff --git a/message_services/Dockerfile b/message_services/Dockerfile index 30c271410..49fec8aa8 100644 --- a/message_services/Dockerfile +++ b/message_services/Dockerfile @@ -1,6 +1,6 @@ FROM ubuntu:bionic-20210702 -RUN apt-get update && apt-get install -y sudo cmake gcc-7 g++-7 libboost1.65-dev libboost-thread1.65-dev libboost-regex1.65-dev libboost-log1.65-dev libboost-program-options1.65-dev libboost1.65-all-dev libxerces-c-dev libcurl4-openssl-dev autotools-dev automake sqlite3 libsqlite3-dev libpugixml-dev libmysqlclient-dev libgeographic-dev libjsoncpp-dev uuid-dev libusb-dev libusb-1.0-0-dev libftdi-dev swig liboctave-dev gpsd libgps-dev portaudio19-dev libsndfile1-dev libglib2.0-dev libglibmm-2.4-dev libpcre3-dev libsigc++-2.0-dev libxml++2.6-dev libxml2-dev liblzma-dev dpkg-dev libmysqlcppconn-dev libev-dev libuv-dev git build-essential libssl-dev qtbase5-dev qtbase5-dev-tools curl libqhttpengine-dev libgtest-dev libcpprest-dev +RUN apt-get update && apt-get install -y sudo cmake gcc-7 g++-7 libboost1.65-dev libboost-thread1.65-dev libboost-regex1.65-dev libboost-log1.65-dev libboost-program-options1.65-dev libboost1.65-all-dev libxerces-c-dev libcurl4-openssl-dev autotools-dev automake sqlite3 libsqlite3-dev libpugixml-dev libmysqlclient-dev libgeographic-dev libjsoncpp-dev uuid-dev libusb-dev libusb-1.0-0-dev libftdi-dev swig liboctave-dev gpsd libgps-dev portaudio19-dev libsndfile1-dev libglib2.0-dev libglibmm-2.4-dev libpcre3-dev libsigc++-2.0-dev libxml++2.6-dev libxml2-dev liblzma-dev dpkg-dev libmysqlcppconn-dev libev-dev libuv-dev git build-essential libssl-dev qtbase5-dev qtbase5-dev-tools curl libqhttpengine-dev libgtest-dev libcpprest-dev RUN mkdir -p /home/ @@ -10,16 +10,18 @@ RUN echo " ------> Install googletest..." WORKDIR /home/carma-streets/ RUN mkdir -p /home/carma-streets/ext WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/google/googletest/ +RUN git clone https://github.com/google/googletest.git -b v1.13.0 WORKDIR /home/carma-streets/ext/googletest/ -RUN cmake . +RUN mkdir build +WORKDIR /home/carma-streets/ext/googletest/build +RUN cmake .. RUN make -RUN sudo make install +RUN make install # Install librdkafka RUN echo " ------> Install librdkafka..." WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/confluentinc/librdkafka.git +RUN git clone https://github.com/confluentinc/librdkafka.git -b v2.2.0 WORKDIR /home/carma-streets/ext/librdkafka/ RUN cmake -H. -B_cmake_build RUN cmake --build _cmake_build @@ -29,7 +31,7 @@ RUN cmake --build _cmake_build --target install # Install spdlog RUN echo " ------> Install spdlog... " WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/gabime/spdlog.git +RUN git clone https://github.com/gabime/spdlog.git -b v1.12.0 WORKDIR /home/carma-streets/ext/spdlog/ RUN mkdir build WORKDIR /home/carma-streets/ext/spdlog/build @@ -42,6 +44,7 @@ RUN echo " ------> Install rapidjson..." WORKDIR /home/carma-streets/ext RUN git clone https://github.com/Tencent/rapidjson WORKDIR /home/carma-streets/ext/rapidjson/ +RUN git checkout a95e013b97ca6523f32da23f5095fcc9dd6067e5 RUN mkdir build WORKDIR /home/carma-streets/ext/rapidjson/build RUN cmake .. && make -j diff --git a/message_services/test/test_vehicle_status_intent_service.cpp b/message_services/test/test_vehicle_status_intent_service.cpp index 03aab60e6..47b6e4d14 100644 --- a/message_services/test/test_vehicle_status_intent_service.cpp +++ b/message_services/test/test_vehicle_status_intent_service.cpp @@ -4,5 +4,7 @@ TEST(test_vehicle_status_intent_service, initialize) { message_services::services::vehicle_status_intent_service service; + streets_service::streets_configuration::create("/home/carma-streets/message_services/manifest.json"); + ASSERT_FALSE(service.initialize()); } \ No newline at end of file diff --git a/scheduling_service/Dockerfile b/scheduling_service/Dockerfile index 8c9e619a9..58584dbc2 100644 --- a/scheduling_service/Dockerfile +++ b/scheduling_service/Dockerfile @@ -12,7 +12,7 @@ RUN mkdir -p /home/carma-streets/ # Install librdkafka RUN echo " ------> Install librdkafka..." WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/confluentinc/librdkafka.git +RUN git clone https://github.com/confluentinc/librdkafka.git -b v2.2.0 WORKDIR /home/carma-streets/ext/librdkafka/ RUN cmake -H. -B_cmake_build RUN cmake --build _cmake_build @@ -23,6 +23,7 @@ RUN echo " ------> Install rapidjson..." WORKDIR /home/carma-streets/ext RUN git clone https://github.com/Tencent/rapidjson WORKDIR /home/carma-streets/ext/rapidjson/ +RUN git checkout a95e013b97ca6523f32da23f5095fcc9dd6067e5 RUN mkdir build WORKDIR /home/carma-streets/ext/rapidjson/build RUN cmake .. && make -j diff --git a/scripts/readme.md b/scripts/README.md similarity index 62% rename from scripts/readme.md rename to scripts/README.md index 3a26cc60a..7649c6d26 100644 --- a/scripts/readme.md +++ b/scripts/README.md @@ -1,8 +1,16 @@ +# Testing Scripts +## Introduction +The python scripts and json files in this directory are used for mocking message communication to provide an environment in which CARMA Streets Service functionality can be integration tested in isolation. Directions for using the scripts should be included below under the appropriate heading. -Requirements: - pip3 install -r requirements.txt - - +## Prerequisites +Please add any script dependencies into requirements.txt so that they can be easily installed using +```sh +pip3 install -r requirements.txt +``` +## simulate_detections.py +### Description +This script is used to mock Detected Object message sent from sensor integrated with CARMA Streets. Currently this only includes CARLA Lidar sensor integrated under the Vunerable Road User Use Case 1 project. It will currently read `detected_cyclist.json`, `detected_pedestrian.json`, and `detected_truck.json` and publish those messages at 10 hz to the detections kafka topic. This script is used to provide input to the **Sensor Data Sharing Service** to generate J2735 SDSMs. To add new detections, create a new json file and update the script to send its content on a given interval. For json format of the [Detected Object Message](../streets_utils/streets_messages/DetectedObjectsMessage.md). +## simulate_vehicle_status_intent_pub_uc1.py Instructions for sending sample status and intent messages for UC1 scheduling logic: - In the simulate_vehicle_status_intent_pub_uc1.py, change the input of the read_json() method on line 23 to @@ -19,7 +27,7 @@ Instructions for sending sample status and intent messages for UC1 scheduling lo * Note: One of the limitation of the python script is that the number of vehicles in each set of updates shall be the same. Otherwise, the time gap between two set of updates will not be correctly applied. - +## simulate_vehicle_status_intent_pub_uc3.py Instructions for sending sample status and intent messages for UC3 scheduling logic: - In the simulate_vehicle_status_intent_pub_uc3.py, change the input of the read_json() method on line 23 to diff --git a/scripts/detected_cyclist.json b/scripts/detected_cyclist.json new file mode 100644 index 000000000..b489f576a --- /dev/null +++ b/scripts/detected_cyclist.json @@ -0,0 +1,79 @@ +{ + "type": "CYCLIST", + "confidence": 0.7, + "sensorId": "sensor1", + "projString": "projection String2", + "objectId": 1, + "position": { + "x": -1.1, + "y": -2.0, + "z": -3.2 + }, + "positionCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "velocity": { + "x": 1.0, + "y": 1.0, + "z": 1.0 + }, + "velocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "angularVelocity": { + "x": 0.1, + "y": 0.2, + "z": 0.3 + }, + "angularVelocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "size": { + "length": 2.0, + "height": 1.0, + "width": 0.5 + }, + "timestamp": 1230 + } \ No newline at end of file diff --git a/scripts/detected_pedestrian.json b/scripts/detected_pedestrian.json new file mode 100644 index 000000000..27ff66afe --- /dev/null +++ b/scripts/detected_pedestrian.json @@ -0,0 +1,79 @@ +{ + "type": "PEDESTRIAN", + "confidence": 0.7, + "sensorId": "sensor1", + "projString": "projection String2", + "objectId": 2, + "position": { + "x": -1.1, + "y": -2.0, + "z": -3.2 + }, + "positionCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "velocity": { + "x": 0.2, + "y": 0.5, + "z": 0.0 + }, + "velocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "angularVelocity": { + "x": 0.1, + "y": 0.2, + "z": 0.3 + }, + "angularVelocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "size": { + "length": 0.5, + "height": 2.0, + "width": 0.5 + }, + "timestamp": 1230 + } \ No newline at end of file diff --git a/scripts/detected_truck.json b/scripts/detected_truck.json new file mode 100644 index 000000000..df3caebb4 --- /dev/null +++ b/scripts/detected_truck.json @@ -0,0 +1,79 @@ +{ + "type": "TRUCK", + "confidence": 0.7, + "sensorId": "sensor1", + "projString": "projection String2", + "objectId": 3, + "position": { + "x": -1.1, + "y": -2.0, + "z": -3.2 + }, + "positionCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "velocity": { + "x": 0.2, + "y": 0.5, + "z": 0.0 + }, + "velocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "angularVelocity": { + "x": 0.1, + "y": 0.2, + "z": 0.3 + }, + "angularVelocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "size": { + "length": 0.5, + "height": 2.0, + "width": 0.5 + }, + "timestamp": 1230 + } \ No newline at end of file diff --git a/scripts/phase_control_schedule.json b/scripts/phase_control_schedule.json new file mode 100644 index 000000000..168f687d1 --- /dev/null +++ b/scripts/phase_control_schedule.json @@ -0,0 +1,137 @@ +{ + "MsgType": "Schedule", + "Schedule": [ + { + "commandEndTime": 0, + "commandPhase": 2, + "commandStartTime": 0, + "commandType": "hold" + }, + { + "commandEndTime": 24, + "commandPhase": 4, + "commandStartTime": 5, + "commandType": "hold" + }, + { + "commandEndTime": 44, + "commandPhase": 2, + "commandStartTime": 29, + "commandType": "hold" + }, + { + "commandEndTime": 64, + "commandPhase": 4, + "commandStartTime": 49, + "commandType": "hold" + }, + { + "commandEndTime": 12, + "commandPhase": 2, + "commandStartTime": 11, + "commandType": "forceoff" + }, + { + "commandEndTime": 69, + "commandPhase": 4, + "commandStartTime": 68, + "commandType": "forceoff" + }, + { + "commandEndTime": 109, + "commandPhase": 2, + "commandStartTime": 108, + "commandType": "forceoff" + }, + { + "commandEndTime": 129, + "commandPhase": 4, + "commandStartTime": 128, + "commandType": "forceoff" + }, + { + "commandEndTime": 23, + "commandPhase": 4, + "commandStartTime": 0, + "commandType": "call_veh" + }, + { + "commandEndTime": 133, + "commandPhase": 1, + "commandStartTime": 0, + "commandType": "omit_veh" + }, + { + "commandEndTime": 133, + "commandPhase": 3, + "commandStartTime": 0, + "commandType": "omit_veh" + }, + { + "commandEndTime": 0, + "commandPhase": 6, + "commandStartTime": 0, + "commandType": "hold" + }, + { + "commandEndTime": 24, + "commandPhase": 7, + "commandStartTime": 5, + "commandType": "hold" + }, + { + "commandEndTime": 44, + "commandPhase": 6, + "commandStartTime": 29, + "commandType": "hold" + }, + { + "commandEndTime": 64, + "commandPhase": 7, + "commandStartTime": 49, + "commandType": "hold" + }, + { + "commandEndTime": 12, + "commandPhase": 6, + "commandStartTime": 11, + "commandType": "forceoff" + }, + { + "commandEndTime": 69, + "commandPhase": 7, + "commandStartTime": 68, + "commandType": "forceoff" + }, + { + "commandEndTime": 109, + "commandPhase": 6, + "commandStartTime": 108, + "commandType": "forceoff" + }, + { + "commandEndTime": 129, + "commandPhase": 7, + "commandStartTime": 128, + "commandType": "forceoff" + }, + { + "commandEndTime": 23, + "commandPhase": 7, + "commandStartTime": 0, + "commandType": "call_veh" + }, + { + "commandEndTime": 133, + "commandPhase": 5, + "commandStartTime": 0, + "commandType": "omit_veh" + }, + { + "commandEndTime": 133, + "commandPhase": 8, + "commandStartTime": 0, + "commandType": "omit_veh" + } + ] + } \ No newline at end of file diff --git a/scripts/phase_control_schedule_clear.json b/scripts/phase_control_schedule_clear.json new file mode 100644 index 000000000..629e2ff4a --- /dev/null +++ b/scripts/phase_control_schedule_clear.json @@ -0,0 +1,4 @@ +{ + "MsgType": "Schedule", + "Schedule": "Clear" + } \ No newline at end of file diff --git a/scripts/phase_control_schedule_simple.json b/scripts/phase_control_schedule_simple.json new file mode 100644 index 000000000..6875345c4 --- /dev/null +++ b/scripts/phase_control_schedule_simple.json @@ -0,0 +1,131 @@ +{ + "MsgType": "Schedule", + "Schedule": [ + { + "commandStartTime": 5000, + "commandPhase": 1, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 5000, + "commandPhase": 2, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 5000, + "commandPhase": 3, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 5000, + "commandPhase": 4, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 5000, + "commandPhase": 5, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 5000, + "commandPhase": 6, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 5000, + "commandPhase": 7, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 5000, + "commandPhase": 8, + "commandEndTime": 10000, + "commandType": "omit_ped" + }, + { + "commandStartTime": 15000, + "commandPhase": 1, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 15000, + "commandPhase": 2, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 15000, + "commandPhase": 3, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 15000, + "commandPhase": 4, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 15000, + "commandPhase": 5, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 15000, + "commandPhase": 6, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 15000, + "commandPhase": 7, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 15000, + "commandPhase": 8, + "commandEndTime": 20000, + "commandType": "call_veh" + }, + { + "commandStartTime": 25000, + "commandPhase": 1, + "commandEndTime": 30000, + "commandType": "call_ped" + }, + { + "commandStartTime": 25000, + "commandPhase": 4, + "commandEndTime": 30000, + "commandType": "call_ped" + }, + { + "commandStartTime": 25000, + "commandPhase": 6, + "commandEndTime": 30000, + "commandType": "call_ped" + }, + { + "commandStartTime": 25000, + "commandPhase": 8, + "commandEndTime": 45000, + "commandType": "call_ped" + }, + { + "commandStartTime": 35000, + "commandPhase": 8, + "commandEndTime": 40000, + "commandType": "forceoff" + } + ] +} \ No newline at end of file diff --git a/scripts/simulate_detected_object.py b/scripts/simulate_detected_object.py new file mode 100644 index 000000000..67e08b7e7 --- /dev/null +++ b/scripts/simulate_detected_object.py @@ -0,0 +1,39 @@ +from os import read +from kafka import KafkaProducer +import time +from time import sleep +import json + + +def read_json(json_name): + data = {} + with open(json_name, "r") as json_file: + data = json.load(json_file) + return data + + +if __name__ == "__main__": + producer = KafkaProducer(bootstrap_servers=["127.0.0.1:9092"], + value_serializer=lambda x: json.dumps(x).encode('utf-8')) + obj = time.gmtime(0) + epoch = time.asctime(obj) + print("The epoch is:",epoch) + + # Send a new schedule + curr_time = round(time.time()*1000) + while True : + data = read_json('detected_pedestrian.json') + producer.send('detections', value=data) + print(f'Sent a detection.{data}') + producer.flush() + data = read_json('detected_cyclist.json') + producer.send('detections', value=data) + print(f'Sent a detection.{data}') + data = read_json('detected_truck.json') + producer.send('detections', value=data) + print(f'Sent a detection.{data}') + producer.flush() + time.sleep(0.1) + + + \ No newline at end of file diff --git a/scripts/simulate_phase_control_schedule.py b/scripts/simulate_phase_control_schedule.py new file mode 100644 index 000000000..f1001a444 --- /dev/null +++ b/scripts/simulate_phase_control_schedule.py @@ -0,0 +1,74 @@ +from os import read +from kafka import KafkaProducer +import time +from time import sleep +import json + + +def read_json(json_name): + data = {} + with open(json_name, "r") as json_file: + data = json.load(json_file) + return data + + +if __name__ == "__main__": + producer = KafkaProducer(bootstrap_servers=["127.0.0.1:9092"], + value_serializer=lambda x: json.dumps(x).encode('utf-8')) + obj = time.gmtime(0) + epoch = time.asctime(obj) + print("The epoch is:",epoch) + + # Send a new schedule + curr_time = round(time.time()*1000) + data = read_json('phase_control_schedule_simple.json') + i = 0 + for schedule in data["Schedule"]: + #update start time with the current time + start time from the json file + data["Schedule"][i]["commandStartTime"] = curr_time + data["Schedule"][i]["commandStartTime"] + #update end time with the current time + end time from the json file + data["Schedule"][i]["commandEndTime"] = curr_time + data["Schedule"][i]["commandEndTime"] + i+=1 + producer.send('phase_control_schedule', value=data) + print(f'Sent a phase control schedule.{data}') + producer.flush() + + # Send a clear schedule + print(f'Sleep 10 seconds...') + sleep(10) # sleep seconds + data = read_json('phase_control_schedule_clear.json') + producer.send('phase_control_schedule', value=data) + print('Sent a phase control schedule clear.') + producer.flush() + + # Send a new schedule to replace the clear schedule + print(f'Sleep 5 seconds...') + sleep(5) # sleep seconds + curr_time = round(time.time()*1000) + data = read_json('phase_control_schedule_simple.json') + i = 0 + for schedule in data["Schedule"]: + #update start time with the current time + start time from the json file + data["Schedule"][i]["commandStartTime"] = curr_time + data["Schedule"][i]["commandStartTime"] + #update end time with the current time + end time from the json file + data["Schedule"][i]["commandEndTime"] = curr_time + data["Schedule"][i]["commandEndTime"] + i+=1 + producer.send('phase_control_schedule', value=data) + print(f'Sent a phase control schedule.{data}') + producer.flush() + + # Send a new schedule to replace the old schedule + print(f'Sleep 5 seconds...') + sleep(5) # sleep seconds + curr_time = round(time.time()*1000) + data = read_json('phase_control_schedule_simple.json') + i = 0 + for schedule in data["Schedule"]: + #update start time with the current time + start time from the json file + data["Schedule"][i]["commandStartTime"] = curr_time + data["Schedule"][i]["commandStartTime"] + #update end time with the current time + end time from the json file + data["Schedule"][i]["commandEndTime"] = curr_time + data["Schedule"][i]["commandEndTime"] + i+=1 + producer.send('phase_control_schedule', value=data) + print(f'Sent a phase control schedule.{data}') + producer.flush() diff --git a/sensor_data_sharing_service/CMakeLists.txt b/sensor_data_sharing_service/CMakeLists.txt new file mode 100644 index 000000000..c78bef0f0 --- /dev/null +++ b/sensor_data_sharing_service/CMakeLists.txt @@ -0,0 +1,101 @@ +# Copyright 2019-2023 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +cmake_minimum_required(VERSION 3.10.2) +project(sensor_data_sharing_service LANGUAGES CXX) +# shared_mutex +set(CMAKE_CXX_STANDARD 17) +# GNU standard installation directories +include(GNUInstallDirs) +# Find Packages +find_package(spdlog REQUIRED) +find_package(RapidJSON REQUIRED) +find_package(GTest REQUIRED) +find_package(streets_messages_lib REQUIRED) +find_package(streets_service_base_lib REQUIRED) +find_package(PROJ4 REQUIRED) +find_package(Eigen3 3.3 NO_MODULE) +find_package(lanelet2_core REQUIRED) +find_package(lanelet2_projection REQUIRED) +find_package(lanelet2_io REQUIRED) +find_package(lanelet2_extension REQUIRED) +find_package(carma-clock REQUIRED) +# Add definition for rapidjson to include std::string +add_definitions(-DRAPIDJSON_HAS_STDSTRING=1) +# Add to make trace level logging available +add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) +# Service Library +set(LIBRARY_NAME ${PROJECT_NAME}_lib) +# Service Executable +set(SERVICE_NAME ${PROJECT_NAME}) +######################################################## +# Build Library and Service Executable +######################################################## + +add_library(${LIBRARY_NAME} + src/sensor_data_sharing_service.cpp + src/sensor_configuration_parser.cpp + src/detected_object_to_sdsm_converter.cpp + src/detected_object_enu_to_ned_converter.cpp) + +target_include_directories(${LIBRARY_NAME} + PUBLIC + $ + ${lanelet2_core_INCLUDE_DIRS} + ${lanelet2_io_INCLUDE_DIRS} + ${lanelet2_projection_INCLUDE_DIRS} + ${lanelet2_extension_lib_INCLUDE_DIRS} +) + + +target_link_libraries( ${LIBRARY_NAME} + PUBLIC + spdlog::spdlog + rapidjson + streets_service_base_lib::streets_service_base_lib + streets_utils::streets_messages_lib + Eigen3::Eigen + ${PROJ4_LIBRARIES} + ${lanelet2_core_LIBRARIES} + ${lanelet2_io_LIBRARIES} + ${lanelet2_projection_LIBRARIES} + ${lanelet2_extension_LIBRARIES} +) + +add_executable(${SERVICE_NAME} src/main.cpp) + +target_link_libraries( ${SERVICE_NAME} + PUBLIC + ${LIBRARY_NAME} +) + + +######################## +# Setup Test executable +######################## +enable_testing() +set(TEST_NAME ${SERVICE_NAME}_test) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.hpp test/*.cpp) +set(SOURCES ${SERVICE_NAME} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +add_executable(${TEST_NAME} ${TEST_SOURCES}) +target_link_libraries(${TEST_NAME} + PRIVATE + ${LIBRARY_NAME} + GTest::Main + ) +include(GoogleTest) +gtest_discover_tests( ${TEST_NAME} + DISCOVERY_MODE PRE_TEST + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test +) + diff --git a/sensor_data_sharing_service/Dockerfile b/sensor_data_sharing_service/Dockerfile new file mode 100644 index 000000000..d09192a22 --- /dev/null +++ b/sensor_data_sharing_service/Dockerfile @@ -0,0 +1,22 @@ +FROM usdotfhwastol/streets_service_base_lanelet_aware:carma-system-4.5.0-bionic +COPY ./sensor_data_sharing_service/ /home/carma-streets/sensor_data_sharing_service +COPY ./streets_utils/ /home/carma-streets/streets_utils +COPY ./kafka_clients/ /home/carma-streets/kafka_clients +RUN /home/carma-streets/sensor_data_sharing_service/build.sh +RUN chmod u+x /opt/carma_lanelet2/setup.bash +# Required due to log file hardcoded relative path "../logs" +# (see https://github.com/usdot-fhwa-stol/carma-streets/issues/391 & +# https://github.com/usdot-fhwa-stol/carma-streets/issues/342) +WORKDIR /home/carma-streets/sensor_data_sharing_service/build + +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="sensor_data_sharing_service" +LABEL org.label-schema.description="Image for Sensor Data Sharing Service" +LABEL org.label-schema.vendor="Leidos" +LABEL org.label-schema.version="${VERSION}" +LABEL org.label-schema.url="https://highways.dot.gov/research/research-programs/operations" +LABEL org.label-schema.vcs-url="https://github.com/usdot-fhwa-stol/carma-streets" +LABEL org.label-schema.vcs-ref=${VCS_REF} +LABEL org.label-schema.build-date=${BUILD_DATE} + +ENTRYPOINT ["/bin/bash", "-c", "source /opt/carma_lanelet2/setup.bash && /home/carma-streets/sensor_data_sharing_service/build/sensor_data_sharing_service"] \ No newline at end of file diff --git a/sensor_data_sharing_service/README.md b/sensor_data_sharing_service/README.md new file mode 100644 index 000000000..32ebde926 --- /dev/null +++ b/sensor_data_sharing_service/README.md @@ -0,0 +1,22 @@ +# Sensor Data Sharing Service +## Introduction +This is the **CARMA Streets** service responsible for consuming [detected_objects_msg](../streets_utils/streets_messages/DetectedObjectsMessage.md) and publishing [sensor_data_sharing_msg](../streets_utils/streets_messages/SensorDataSharingMessage.md) in accordance with the J3224 specification. This service is built on the [streets_service_base_lanelet_aware](../streets_service_base_lanelet_aware/README.md) base image to include lanelet2 dependencies used for map coordinate frame translations. +## Configuration Parameters +**TODO** Saving this part for once I finalize the implementation +> [!IMPORTANT]\ +> Initial implementation of this service will not support the BSM functionality described in the J3224 specification and will also not support any data fusion from multiple infrastructure sensors. +## Covariance to confidence conversion +![image](https://github.com/usdot-fhwa-stol/carma-streets/assets/77466294/b5f4d827-7768-40ba-8674-3115ff4a338c) + +For covariance to confidence/accuracy translation assumed the following. All confidence/accuracy measurements in SDSM assume 95% confidence interval ( from J3224 documentation). Assuming normal distribution of measurements. Diagonal in covariance matrix is variance for each component. SD (Standard deviation) is square root of variance. 2 standard deviations covers 95 % of sample in normal distribution +![image](https://github.com/usdot-fhwa-stol/carma-streets/assets/77466294/4a6e9875-2f87-4a8f-b1a0-33a6769439ac) + +## Sensor Data Sharing speed and heading +For heading and speed we use following coordinate frame (NED) as described in the J3224 specifications. + +![Alt text](docs/image.png) + +We assume incoming detections are in the ENU cordinate frame. The `detected_object_enu_to_ned_converter` handles conversion of position and velocity in detection to NED coordinate frame. + +![Alt text](docs/ned_enu.png) + diff --git a/sensor_data_sharing_service/build.sh b/sensor_data_sharing_service/build.sh new file mode 100755 index 000000000..7510f071b --- /dev/null +++ b/sensor_data_sharing_service/build.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# Copyright (C) 2018-2020 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +# Build script to build Sensor Data Sharing Service +set -e +source /opt/carma_lanelet2/setup.bash + + +COVERAGE_FLAGS="" + +# make install for these subdirectories +MAKE_INSTALL_DIRS=( + "streets_utils/json_utils" + "streets_utils/streets_messages" + "streets_utils/streets_service_configuration" + "kafka_clients" + "streets_utils/streets_service_base" +) + +# only make for these subdirectories +MAKE_ONLY_DIRS=( + "sensor_data_sharing_service" +) + +for DIR in "${MAKE_INSTALL_DIRS[@]}" "${MAKE_ONLY_DIRS[@]}"; do + cd /home/carma-streets/"$DIR" + cmake -Bbuild -DCMAKE_CXX_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_C_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DCMAKE_BUILD_TYPE="Debug" -DCMAKE_PREFIX_PATH="/opt/carma/cmake/;/opt/carma_lanelet2/" + cmake --build build + for MAKE_INSTALL_DIR in "${MAKE_INSTALL_DIRS[@]}"; do + if [ "$DIR" == "$MAKE_INSTALL_DIR" ]; then + cmake --install build + fi + done +done +ldconfig \ No newline at end of file diff --git a/sensor_data_sharing_service/docs/image.png b/sensor_data_sharing_service/docs/image.png new file mode 100644 index 000000000..9d5fca349 Binary files /dev/null and b/sensor_data_sharing_service/docs/image.png differ diff --git a/sensor_data_sharing_service/docs/ned_enu.png b/sensor_data_sharing_service/docs/ned_enu.png new file mode 100644 index 000000000..c4e246e89 Binary files /dev/null and b/sensor_data_sharing_service/docs/ned_enu.png differ diff --git a/sensor_data_sharing_service/include/detected_object_enu_to_ned_converter.hpp b/sensor_data_sharing_service/include/detected_object_enu_to_ned_converter.hpp new file mode 100644 index 000000000..9ad8a034f --- /dev/null +++ b/sensor_data_sharing_service/include/detected_object_enu_to_ned_converter.hpp @@ -0,0 +1,25 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include + +namespace sensor_data_sharing_service { + /** + * @brief Converts detected_objects_msg from ENU coordinate frame to NED. + * @param msg + * @return Returns detected_objects_msg in NED coordinate frame. + */ + streets_utils::messages::detected_objects_msg::detected_objects_msg detected_object_enu_to_ned(const streets_utils::messages::detected_objects_msg::detected_objects_msg &msg ); +} \ No newline at end of file diff --git a/sensor_data_sharing_service/include/detected_object_to_sdsm_converter.hpp b/sensor_data_sharing_service/include/detected_object_to_sdsm_converter.hpp new file mode 100644 index 000000000..36d6861c5 --- /dev/null +++ b/sensor_data_sharing_service/include/detected_object_to_sdsm_converter.hpp @@ -0,0 +1,128 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include +#include //include all types plus i/o +#include +#include +#include +#include + +namespace sensor_data_sharing_service { + /** + * @brief Map of detection types to SDSM object types. + */ + inline const std::map> sdsm_object_types = { + {"CAR",streets_utils::messages::sdsm::object_type::VEHICLE }, + {"VAN",streets_utils::messages::sdsm::object_type::VEHICLE }, + {"TRUCK",streets_utils::messages::sdsm::object_type::VEHICLE }, + {"PEDESTRIAN",streets_utils::messages::sdsm::object_type::VRU }, + {"CYCLIST",streets_utils::messages::sdsm::object_type::VRU }, + {"MOTORCYCLE",streets_utils::messages::sdsm::object_type::VRU }, + }; + + inline const int MILLISECONDS_TO_MICROSECONDS = 1000; + + inline const int SECONDS_TO_MILLISECONDS = 1000; + + inline const int METERS_TO_CM = 100; + + inline const int METERS_TO_5_CM = 20; + + inline const int METERS_TO_10_CM = 10; + + inline const int METERS_PER_SECOND_TO_2_CM_PER_SECOND = 50; + /** + * @brief convert epoch millisecond timestamp to sdsm timestamp object + * @return streets_utils::messages::sdsm::time_stamp. + */ + streets_utils::messages::sdsm::time_stamp to_sdsm_timestamp(const uint64_t _epoch_time_ms); + /** + * @brief convert detected_object_msg to sdsm detection_object_data. + * @note method assumes detected_object_msg is in NED coordinate frame (NED is required for SDSM based on J3223 specification) + * @return streets_utils::messages::sdsm::detected_object_data. + */ + streets_utils::messages::sdsm::detected_object_data to_detected_object_data(const streets_utils::messages::detected_objects_msg::detected_objects_msg &msg, uint64_t sdsm_message_timestamp); + /** + * @brief convert string detection type to sdsm object_type. + * @param detection_type string detection classification for detection message. + * @return streets_utils::messages::sdsm::object_type + */ + streets_utils::messages::sdsm::object_type to_object_type(const std::string &detection_type); + /** + * @brief Convert XY velocity into 2 dimensional heading measured in 0.0125 degree units + * @param velocity vector of velocity + * @return 2 dimensional heading in units of 0.0125 degrees + */ + unsigned int to_heading(const streets_utils::messages::detected_objects_msg::vector_3d &velocity); + /** + * @brief Convert position covariance to position confidence set which includes positionXY confidence + * and position z confidence. + * @param _position_covariance + * @return position confidence set. + */ + streets_utils::messages::sdsm::position_confidence_set to_position_confidence_set( const std::vector> &_position_covariance); + /** + * @brief Convert velocity covariance to xy speed confidence. + * @param velocity_covariance + * @return xy speed confidence + */ + streets_utils::messages::sdsm::speed_confidence to_xy_speed_confidence(const std::vector> &velocity_covariance); + /** + * @brief Convert velocity covariance to z speed confidence. + * @param velocity_covariance + * @return z speed confidence + */ + streets_utils::messages::sdsm::speed_confidence to_z_speed_confidence(const std::vector> &velocity_covariance); + /** + * @brief Convert accuracy value to speed confidence enumeration. To minimize error in conversion, method + * rounds up/down to the nearest enum value + * @param accuracy + * @return speed confidence enumeration + */ + streets_utils::messages::sdsm::speed_confidence to_speed_confidence(const double accuracy); + /** + * @brief Convert angular velocity covariance to angular velocity confidence. + * @param angular_velocity_covariance + * @return angular velocity confidence + */ + streets_utils::messages::sdsm::angular_velocity_confidence to_yaw_rate_confidence( const std::vector> &angular_velocity_covariance ); + /** + * @brief Convert accuracy value to position confidence enumeration. To minimize error in conversion, method + * rounds up/down to the nearest enum value + * @param accuracy + * @return position confidence enumeration + */ + streets_utils::messages::sdsm::position_confidence to_position_confidence(const double accuracy); + /** + * @brief Convert accuracy value to angular velocity confidence enumeration. To minimize error in conversion, method + * rounds up/down to the nearest enum value + * @param accuracy + * @return angular velocity confidence + */ + streets_utils::messages::sdsm::angular_velocity_confidence to_angular_velocity_confidence(const double accuracy); + + /** + * @brief Convert yaw rate in radians per second to 100ths of a degree per second + * @param yaw_rate_radians_per_second + * @return yaw rate in 0.01 degrees/second. + */ + int to_yaw_rate( const double yaw_rate_radians_per_second ); + + + + +} \ No newline at end of file diff --git a/sensor_data_sharing_service/include/sensor_configuration_parser.hpp b/sensor_data_sharing_service/include/sensor_configuration_parser.hpp new file mode 100644 index 000000000..8c4c49e9b --- /dev/null +++ b/sensor_data_sharing_service/include/sensor_configuration_parser.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +// Lanelet2 libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sensor_data_sharing_service{ + /** + * @brief Method to parse sensor configuration json file. + * @param filepath absolute or relative file path. + * @param sensor_id ID of sensor to get location from. + * @return cartesian location of sensor described in json configuration file. + */ + lanelet::BasicPoint3d parse_sensor_location( const std::string &filepath, const std::string &sensor_id ); +} \ No newline at end of file diff --git a/sensor_data_sharing_service/include/sensor_data_sharing_service.hpp b/sensor_data_sharing_service/include/sensor_data_sharing_service.hpp new file mode 100644 index 000000000..b69cd953a --- /dev/null +++ b/sensor_data_sharing_service/include/sensor_data_sharing_service.hpp @@ -0,0 +1,155 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// Lanelet2 libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "sensor_configuration_parser.hpp" +#include "detected_object_to_sdsm_converter.hpp" +#include "detected_object_enu_to_ned_converter.hpp" + + +namespace sensor_data_sharing_service { + + class sds_service : public streets_service::streets_service { + private: + /** + * @brief Kafka producer for SDSM JSON + */ + std::shared_ptr sdsm_producer; + /* + * @brief Kafka consumer for consuming Detected Object JSON + */ + std::shared_ptr detection_consumer; + /** + * @brief Map of detected objects. New detections of existing objects will replace old detections. + */ + std::map detected_objects; + /** + * @brief Mutex for thread safe operations on detected objects map. + */ + std::shared_mutex detected_objects_lock; + /** + * @brief Lanelet2 Map pointer + */ + lanelet::LaneletMapPtr map_ptr; + + /** + * @brief WSG84 Map projection + */ + std::unique_ptr map_projector; + + /** + * @brief Location of sensor.This is also the sensor's coordinate frame orignin meaning all offsets + * are interpreted relative to this location. + */ + lanelet::GPSPoint sdsm_reference_point; + + /** + * @brief Infrastructure ID used as source ID for SDSM broadcast + */ + std::string _infrastructure_id; + + /** + * @brief Message count for SDSM + */ + uint8_t _message_count = 0; + + /** + * @brief Initialize Kafka consumers and producers for sensor data sharing service. + * @return true if successful and false if unsuccessful. + */ + bool initialize_kafka_consumers_producers( ); + /** + * @brief Loop to consume detections from kafka detection consumer. Will terminate if kafka consumer + * is no longer running + * + * @throws std::runtime exception if detection_consumer == nullptr + */ + void consume_detections(); + /** + * @brief Loop to produce sensor data sharing messages from detected_objects. Loop will populate sensor data sharing message with + * most recent detection information, publish message and clear detected object map. Will terminate if kafka producer is no longer + * running. + * + * @throws std::runtime exception if sdsm_producer == nullptr + */ + void produce_sdsms(); + /** + * @brief Method to read lanelet2 map. + * @param filepath to lanelet2 osm file. + * @return true if successful. + */ + bool read_lanelet_map(const std::string &filepath); + + /** + * @brief Method to create SDSM from detected objects. + * @return sensor_data_sharing_msg created from detected objects. + */ + streets_utils::messages::sdsm::sensor_data_sharing_msg create_sdsm(); + + + public: + sds_service() = default; + + ~sds_service(); + + /** + * @brief Method to initialize the sds_service. + * + * @return true if successful. + * @return false if not successful. + */ + bool initialize() override; + + /** + * @brief Method to start all threads included in the tsc_service. + */ + void start() override; + + FRIEND_TEST(sensorDataSharingServiceTest, consumeDetections); + FRIEND_TEST(sensorDataSharingServiceTest, produceSdsms); + FRIEND_TEST(sensorDataSharingServiceTest, readLanelet2Map); + }; + streets_utils::messages::sdsm::position_3d to_position_3d(const lanelet::GPSPoint &ref_position); + +} \ No newline at end of file diff --git a/sensor_data_sharing_service/manifest.json b/sensor_data_sharing_service/manifest.json new file mode 100644 index 000000000..eb7baf7f8 --- /dev/null +++ b/sensor_data_sharing_service/manifest.json @@ -0,0 +1,42 @@ +{ + "service_name": "sensor_data_sharing_service", + "loglevel": "debug", + "configurations": [ + { + "name": "sdsm_producer_topic", + "value": "v2xhub_sdsm_sub", + "description": "Kafka topic to which the sensor data sharing service will produce SDSMs.", + "type": "STRING" + }, + { + "name": "detection_consumer_topic", + "value": "v2xhub_sim_sensor_detected_object", + "description":"Kafka topic from which the sensor data sharing service will consume Detected Objects.", + "type": "STRING" + }, + { + "name": "sensor_configuration_file_path", + "value": "/home/carma-streets/sensor_configurations/sensors.json", + "description": "Path to sensor configuration information that includes sensor location and orientation.", + "type": "STRING" + }, + { + "name": "bootstrap_server", + "value": "127.0.0.1:9092", + "description": "Kafka Broker Server Address.", + "type": "STRING" + }, + { + "name": "sensor_id", + "value": "sensor_1", + "description": "Unique id of sensor for which to publish SDSMS", + "type": "STRING" + }, + { + "name": "sdsm_geo_reference", + "value": "+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs", + "description": "Geo reference used for reference frame for Sensor Data Sharing messges broadcast", + "type": "STRING" + } + ] +} \ No newline at end of file diff --git a/sensor_data_sharing_service/src/detected_object_enu_to_ned_converter.cpp b/sensor_data_sharing_service/src/detected_object_enu_to_ned_converter.cpp new file mode 100644 index 000000000..a2c3264fe --- /dev/null +++ b/sensor_data_sharing_service/src/detected_object_enu_to_ned_converter.cpp @@ -0,0 +1,29 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "detected_object_enu_to_ned_converter.hpp" + +namespace sensor_data_sharing_service { + + streets_utils::messages::detected_objects_msg::detected_objects_msg detected_object_enu_to_ned(const streets_utils::messages::detected_objects_msg::detected_objects_msg &msg ) { + streets_utils::messages::detected_objects_msg::detected_objects_msg ned_detection(msg); + ned_detection._position._x = msg._position._y; + ned_detection._position._y = msg._position._x; + ned_detection._position._z = -msg._position._z; + ned_detection._velocity._x = msg._velocity._y; + ned_detection._velocity._y = msg._velocity._x; + ned_detection._velocity._z = -msg._velocity._z; + return ned_detection; + } +} + diff --git a/sensor_data_sharing_service/src/detected_object_to_sdsm_converter.cpp b/sensor_data_sharing_service/src/detected_object_to_sdsm_converter.cpp new file mode 100644 index 000000000..0ae64ff47 --- /dev/null +++ b/sensor_data_sharing_service/src/detected_object_to_sdsm_converter.cpp @@ -0,0 +1,283 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "detected_object_to_sdsm_converter.hpp" + +namespace sensor_data_sharing_service{ + + streets_utils::messages::sdsm::time_stamp to_sdsm_timestamp(const uint64_t _epoch_time_ms) { + streets_utils::messages::sdsm::time_stamp sdsm_timestamp; + + // From millisecond time stamp + boost::posix_time::ptime posix_time = boost::posix_time::from_time_t(_epoch_time_ms/SECONDS_TO_MILLISECONDS) + + boost::posix_time::millisec( _epoch_time_ms % SECONDS_TO_MILLISECONDS); + sdsm_timestamp.year = posix_time.date().year(); + sdsm_timestamp.month = posix_time.date().month(); + sdsm_timestamp.day = posix_time.date().day(); + + sdsm_timestamp.hour = (unsigned int) posix_time.time_of_day().hours(); + sdsm_timestamp.minute = (unsigned int) posix_time.time_of_day().minutes(); + // Milliseconds of the current minute. The SDSM field is named seconds but is in the unit of milliseconds (see DDateTime from J2735). + // Fractional_seconds returns microseconds from the current second since default time resolution is microseconds + sdsm_timestamp.second = (unsigned int) (posix_time.time_of_day().seconds()*SECONDS_TO_MILLISECONDS + posix_time.time_of_day().fractional_seconds()/MILLISECONDS_TO_MICROSECONDS); + return sdsm_timestamp; + } + + streets_utils::messages::sdsm::detected_object_data to_detected_object_data(const streets_utils::messages::detected_objects_msg::detected_objects_msg &msg, uint64_t sdsm_message_timestamp ) { + streets_utils::messages::sdsm::detected_object_data detected_object; + detected_object._detected_object_common_data._object_type = to_object_type(msg._type); + if (detected_object._detected_object_common_data._object_type == streets_utils::messages::sdsm::object_type::VEHICLE ) { + streets_utils::messages::sdsm::detected_vehicle_data optional_data; + // Size in cm + streets_utils::messages::sdsm::vehicle_size veh_size; + veh_size._length= static_cast(msg._size._length*METERS_TO_CM); + veh_size._width= static_cast(msg._size._width*METERS_TO_CM); + optional_data._size = veh_size; + // Height in 5 cm + optional_data._vehicle_height = static_cast(msg._size._height * METERS_TO_5_CM); + + + detected_object._detected_object_optional_data = optional_data; + } + else if ( detected_object._detected_object_common_data._object_type == streets_utils::messages::sdsm::object_type::VRU ) { + streets_utils::messages::sdsm::detected_vru_data optional_data; + // Populate Optional VRU data + detected_object._detected_object_optional_data = optional_data; + } + else if (detected_object._detected_object_common_data._object_type == streets_utils::messages::sdsm::object_type::UNKNOWN ){ + streets_utils::messages::sdsm::detected_obstacle_data optional_data; + // size dimensions in units of 0.1 m + streets_utils::messages::sdsm::obstacle_size obs_size; + obs_size._length = static_cast(msg._size._length*METERS_TO_10_CM); + obs_size._width = static_cast(msg._size._width*METERS_TO_10_CM); + obs_size._height = static_cast(msg._size._height*METERS_TO_10_CM); + optional_data._size = obs_size; + + detected_object._detected_object_optional_data = optional_data; + + } + // Used to convey an offset in time relative to the sDSMTimeStamp associated with the reference position. Negative values indicate + // the provided detected object characteristics refer to a point in time after the sDSMTimeStamp + detected_object._detected_object_common_data._time_measurement_offset = static_cast(sdsm_message_timestamp - msg._timestamp); + detected_object._detected_object_common_data._classification_confidence = static_cast(msg._confidence*100); + // TODO: Change Detected Object ID to int + detected_object._detected_object_common_data._object_id = msg._object_id; + // Units are 0.1 m + detected_object._detected_object_common_data._position_offset._offset_x = static_cast(msg._position._x*METERS_TO_10_CM); + detected_object._detected_object_common_data._position_offset._offset_y = static_cast(msg._position._y*METERS_TO_10_CM); + detected_object._detected_object_common_data._position_offset._offset_z = static_cast(msg._position._z*METERS_TO_10_CM); + // Position Confidence + detected_object._detected_object_common_data._pos_confidence = to_position_confidence_set(msg._position_covariance); + // Units are 0.02 m/s + detected_object._detected_object_common_data._speed = static_cast(std::hypot( msg._velocity._x* METERS_PER_SECOND_TO_2_CM_PER_SECOND, msg._velocity._y* METERS_PER_SECOND_TO_2_CM_PER_SECOND)); + // Speed confidence + detected_object._detected_object_common_data._speed_confidence = to_xy_speed_confidence(msg._velocity_covariance); + // Speed Z + detected_object._detected_object_common_data._speed_z = static_cast(fabs(msg._velocity._z)* METERS_PER_SECOND_TO_2_CM_PER_SECOND); + // Speed Z confidence + detected_object._detected_object_common_data._speed_z_confidence = to_z_speed_confidence(msg._velocity_covariance); + // Heading + detected_object._detected_object_common_data._heading = to_heading(msg._velocity); + // TODO: how to calculate heading confidence without orientation covariance + // Possible approximation is velocity covariance since we are using that currently + // Currently hard coding value + detected_object._detected_object_common_data._heading_confidence = streets_utils::messages::sdsm::heading_confidence::PREC_0_1_deg; + // Yaw rate + detected_object._detected_object_common_data._acceleration_4_way->_yaw_rate = to_yaw_rate(msg._angular_velocity._z); + // Yaw rate confidence + detected_object._detected_object_common_data._yaw_rate_confidence = to_yaw_rate_confidence(msg._angular_velocity_covariance); + + + return detected_object; + } + + streets_utils::messages::sdsm::object_type to_object_type(const std::string &detection_type){ + if ( sdsm_object_types.find(detection_type) != sdsm_object_types.end()) { + return sdsm_object_types.at(detection_type); + } + return streets_utils::messages::sdsm::object_type::UNKNOWN; + } + + streets_utils::messages::sdsm::position_confidence_set to_position_confidence_set(const std::vector> &_position_covariance) { + auto x_variance = _position_covariance[0][0]; + auto y_variance = _position_covariance[1][1]; + auto z_variance = _position_covariance[2][2]; + // Assuming normal distribution and 95 % confidence interval (From J3224 Specification documentation of all accuracy/confidence measures) + // +/- 2*variance to achieve 95% confidence interval. + // TODO: If position variance x is different from y how do we handle this? For now assuming X variance is the same as Y and justing using + // X + // Multiply variance by 2 get 95% confidence interval for normal distribution + auto xy_accuracy = sqrt(x_variance) * 2 ; + auto z_accuracy = sqrt(z_variance) * 2; + streets_utils::messages::sdsm::position_confidence_set position_confidence_set; + position_confidence_set._position_confidence = to_position_confidence(xy_accuracy); + position_confidence_set._elevation_confidence = to_position_confidence(z_accuracy); + return position_confidence_set; + } + + streets_utils::messages::sdsm::position_confidence to_position_confidence(const double accuracy) { + // Check to see if accuracy is greater than or equal to half way between to confidence enumeration + if ( accuracy >= 350) { + return streets_utils::messages::sdsm::position_confidence::A_500M; + } + else if (accuracy >= 150) { + return streets_utils::messages::sdsm::position_confidence::A_200M; + } + else if (accuracy >= 75) { + return streets_utils::messages::sdsm::position_confidence::A_100M; + } + else if (accuracy >= 35) { + return streets_utils::messages::sdsm::position_confidence::A_50M; + } + else if (accuracy >= 15) { + return streets_utils::messages::sdsm::position_confidence::A_20M; + } + else if (accuracy >= 7.5) { + return streets_utils::messages::sdsm::position_confidence::A_10M; + } + else if (accuracy >= 3.5) { + return streets_utils::messages::sdsm::position_confidence::A_5M; + } + else if (accuracy >= 1.5) { + return streets_utils::messages::sdsm::position_confidence::A_2M; + } + else if (accuracy >= 0.75) { + return streets_utils::messages::sdsm::position_confidence::A_1M; + } + else if (accuracy >= 0.35) { + return streets_utils::messages::sdsm::position_confidence::A_50CM; + } + else if (accuracy >= 0.15) { + return streets_utils::messages::sdsm::position_confidence::A_20CM; + } + else if (accuracy >= 0.075) { + return streets_utils::messages::sdsm::position_confidence::A_10CM; + } + else if (accuracy >= 0.035) { + return streets_utils::messages::sdsm::position_confidence::A_5CM; + } + else if (accuracy >= 0.015) { + return streets_utils::messages::sdsm::position_confidence::A_2CM; + } + else { + // This is the lowest position confidence the SDSM can reflect + return streets_utils::messages::sdsm::position_confidence::A_1CM; + } + + } + + unsigned int to_heading(const streets_utils::messages::detected_objects_msg::vector_3d &velocity){ + if ( std::abs(velocity._x) < 0.01 && std::abs(velocity._y) < 0.01) { + return 0; + } + auto heading_radians = std::atan2(velocity._y,velocity._x); + auto heading_degrees = heading_radians*(180/M_PI); + // If angle is negative 360 + (-negative angle) + if ( heading_degrees < 0 ) { + heading_degrees = 360 + heading_degrees; + } + // in units (x) = 0.0125 degrees + // 28800 x = 360 degrees + return static_cast(heading_degrees * (28800/360)); + } + + streets_utils::messages::sdsm::speed_confidence to_xy_speed_confidence(const std::vector> &velocity_covariance) { + auto x_variance = velocity_covariance[0][0]; + auto y_variance = velocity_covariance[1][1]; + // Assuming normal distribution and 95 % confidence interval (From J3224 Specification documentation of all accuracy/confidence measures) + // +/- 2*variance to achieve 95% confidence interval. + // TODO: If position variance x is different from y how do we handle this? For now assuming X variance is the same as Y and justing using + // X + // Multiply variance by 2 get 95% confidence interval for normal distribution + auto xy_accuracy = sqrt(x_variance) * 2 ; + return to_speed_confidence(xy_accuracy); + } + + streets_utils::messages::sdsm::speed_confidence to_z_speed_confidence(const std::vector> &velocity_covariance) { + auto z_variance = velocity_covariance[2][2]; + // Assuming normal distribution and 95 % confidence interval (From J3224 Specification documentation of all accuracy/confidence measures) + // +/- 2*variance to achieve 95% confidence interval. + + // Multiply variance by 2 get 95% confidence interval for normal distribution + auto z_accuracy = sqrt(z_variance) * 2 ; + return to_speed_confidence(z_accuracy); + } + + streets_utils::messages::sdsm::speed_confidence to_speed_confidence(const double accuracy) { + // Check to see if accuracy is greater than or equal to half way between to confidence enumeration + if ( accuracy >= 55) { + return streets_utils::messages::sdsm::speed_confidence::PREC_100ms; + } + else if (accuracy >= 7.5) { + return streets_utils::messages::sdsm::speed_confidence::PREC_10ms; + } + else if (accuracy >= 3) { + return streets_utils::messages::sdsm::speed_confidence::PREC_5ms; + } + else if (accuracy >= .55) { + return streets_utils::messages::sdsm::speed_confidence::PREC_1ms; + } + else if (accuracy >= .075) { + return streets_utils::messages::sdsm::speed_confidence::PREC_0_1ms; + } + else if (accuracy >= .03) { + return streets_utils::messages::sdsm::speed_confidence::PREC_0_05ms; + } + else { + // This is the lowest speed confidence the SDSM can reflect + return streets_utils::messages::sdsm::speed_confidence::PREC_0_01ms; + } + } + + + streets_utils::messages::sdsm::angular_velocity_confidence to_yaw_rate_confidence( const std::vector> &angular_velocity_covariance ){ + auto z_variance = angular_velocity_covariance[2][2]; + auto z_accuracy = sqrt(z_variance) * 2 ; + return to_angular_velocity_confidence(z_accuracy); + } + + + streets_utils::messages::sdsm::angular_velocity_confidence to_angular_velocity_confidence(const double accuracy) { + if ( accuracy >= 55) { + return streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_100; + } + else if (accuracy >= 7.5) { + return streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_10; + } + else if (accuracy >= 3) { + return streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_05; + } + else if (accuracy >= .55) { + return streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_01; + } + else if (accuracy >= .075) { + return streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_1; + } + else if (accuracy >= .03) { + return streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_05; + } + else { + // This is the lowest speed confidence the SDSM can reflect + return streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_01; + } + } + + int to_yaw_rate( const double yaw_rate_radians_per_second ) { + // Return in units of 0.01 degrees per second; + return static_cast( yaw_rate_radians_per_second * (180/(M_PI)) * 100); + } + + + + +} \ No newline at end of file diff --git a/sensor_data_sharing_service/src/main.cpp b/sensor_data_sharing_service/src/main.cpp new file mode 100644 index 000000000..1ddd585ae --- /dev/null +++ b/sensor_data_sharing_service/src/main.cpp @@ -0,0 +1,42 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include +#include "streets_configuration.h" +#include "sensor_data_sharing_service.hpp" + +int main(int argc, char **argv) +{ + + sensor_data_sharing_service::sds_service service; + try { + if (service.initialize()){ + service.start(); + } + else { + SPDLOG_ERROR("TSC Service Initialization failed!"); + } + } + catch ( const std::exception &e) { + SPDLOG_ERROR("Exception Encountered : {0}" , e.what()); + exit(1); + } + catch ( ... ) { + SPDLOG_CRITICAL("Unknown error occured."); + exit(1); + } + + return 0; +} \ No newline at end of file diff --git a/sensor_data_sharing_service/src/sensor_configuration_parser.cpp b/sensor_data_sharing_service/src/sensor_configuration_parser.cpp new file mode 100644 index 000000000..c8de50679 --- /dev/null +++ b/sensor_data_sharing_service/src/sensor_configuration_parser.cpp @@ -0,0 +1,45 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "sensor_configuration_parser.hpp" + +namespace sensor_data_sharing_service { + + lanelet::BasicPoint3d parse_sensor_location( const std::string &filepath , const std::string &sensor_id ){ + // Parse JSON configuration file + auto doc = streets_utils::json_utils::parse_json_file(filepath); + if (!doc.IsArray()) { + throw streets_utils::json_utils::json_parse_exception("Invadid format for sensor configuration file " + filepath + + ". Sensor configuration file should contain an array of json sensor configurations!"); + } + bool found = false; + lanelet::BasicPoint3d sensor_location; + for (auto itr = doc.Begin(); itr != doc.End(); ++itr) { + // Access the data in the object + auto sensor_config_id = streets_utils::json_utils::parse_string_member("sensorId", itr->GetObject(), true ).value(); + if ( sensor_config_id == sensor_id ) { + found = true; + auto location = streets_utils::json_utils::parse_object_member("location", itr->GetObject(), true ).value(); + sensor_location = lanelet::BasicPoint3d{ + streets_utils::json_utils::parse_double_member("x", location, true ).value(), + streets_utils::json_utils::parse_double_member("y", location, true ).value(), + streets_utils::json_utils::parse_double_member("z", location, true ).value() + }; + } + } + if (!found) { + throw streets_utils::json_utils::json_parse_exception("Did not find sensor with id " + sensor_id + " in sensor configuration file " + filepath + "!"); + } + return sensor_location; + } +} \ No newline at end of file diff --git a/sensor_data_sharing_service/src/sensor_data_sharing_service.cpp b/sensor_data_sharing_service/src/sensor_data_sharing_service.cpp new file mode 100644 index 000000000..675b14a65 --- /dev/null +++ b/sensor_data_sharing_service/src/sensor_data_sharing_service.cpp @@ -0,0 +1,215 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "sensor_data_sharing_service.hpp" + +namespace sensor_data_sharing_service { + + namespace ss = streets_service; + namespace sdsm = streets_utils::messages::sdsm; + namespace detected_objects_message = streets_utils::messages::detected_objects_msg; + sds_service::~sds_service() { + + if (sdsm_producer) + { + SPDLOG_WARN("Stopping SDSM producer!"); + sdsm_producer->stop(); + } + + if(detection_consumer) + { + SPDLOG_WARN("Stopping Detection consumer!"); + detection_consumer->stop(); + } + } + + bool sds_service::initialize() { + if (!streets_service::initialize() ) { + SPDLOG_ERROR("Failed to initialize streets service base!"); + return false; + } + SPDLOG_DEBUG("Intializing Sensor Data Sharing Service"); + const std::string lanlet2_map = streets_service::get_system_config("LANELET2_MAP", "/home/carma-streets/MAP/Intersection.osm"); + if (!read_lanelet_map(lanlet2_map)){ + SPDLOG_ERROR("Failed to read lanlet2 map {0} !", lanlet2_map); + return false; + } + // Read sensor configuration file and get WSG84 location/origin reference frame. + const std::string sensor_config_file = streets_service::get_system_config("SENSOR_JSON_FILE_PATH", "/home/carma-streets/sensor_configurations/sensors.json"); + const std::string sensor_id = ss::streets_configuration::get_string_config("sensor_id"); + const lanelet::BasicPoint3d pose = parse_sensor_location(sensor_config_file, sensor_id); + this->sdsm_reference_point = this->map_projector->reverse(pose); + + // Initialize SDSM Kafka producer + const std::string sdsm_topic = ss::streets_configuration::get_string_config("sdsm_producer_topic"); + const std::string detection_topic = ss::streets_configuration::get_string_config("detection_consumer_topic"); + const std::string sdsm_geo_reference = ss::streets_configuration::get_string_config("sdsm_geo_reference"); + // Get Infrastructure ID for SDSM messages + this->_infrastructure_id = streets_service::get_system_config("INFRASTRUCTURE_ID", ""); + return initialize_kafka_producer(sdsm_topic, sdsm_producer) && initialize_kafka_consumer(detection_topic, detection_consumer); + } + + bool sds_service::read_lanelet_map(const std::string &filepath) { + + try + { + int projector_type = 1; + std::string target_frame; + lanelet::ErrorMessages errors; + // Parse geo reference info from the lanelet map (.osm) + lanelet::io_handlers::AutowareOsmParser::parseMapParams(filepath, &projector_type, &target_frame); + this->map_projector = std::make_unique(target_frame.c_str()); + this->map_ptr = lanelet::load(filepath, *map_projector.get(), &errors); + // + + if (!this->map_ptr->empty()) + { + return true; + } + } + catch (const lanelet::ParseError &ex) + { + SPDLOG_ERROR("Cannot read osm file {0}. Error message: {1} .", filepath, ex.what()); + } + return false; + } + + void sds_service::consume_detections(){ + if ( !detection_consumer ) { + throw std::runtime_error("Detection consumer is null!"); + } + SPDLOG_DEBUG("Attempting to consume detections ..."); + detection_consumer->subscribe(); + while ( detection_consumer->is_running() ) { + try{ + const std::string payload = detection_consumer->consume(1000); + if (payload.length() != 0) + { + auto detected_object = streets_utils::messages::detected_objects_msg::from_json(payload); + // Get delay of detected object + auto delay = static_cast(ss::streets_clock_singleton::time_in_ms()) - static_cast(detected_object._timestamp); + SPDLOG_DEBUG("Detection Delay : {0}ms!", delay); + // if delay is greater than 500 ms skip detection to get more recent data + if ( delay >= 500 ) { + SPDLOG_WARN("Skipping incoming detection at {0}ms is not current or has invalid timestamp of {1}ms!" , ss::streets_clock_singleton::time_in_ms(), detected_object._timestamp ); + continue; + } + // if delay is negative, and service is in simulation mode + // the detection message was processed before time sync message. Wait on time sync message + else if (is_simulation_mode() && delay < 0 ) { + SPDLOG_WARN("Current sim time {0} waiting for sim time {1}ms from detection ...",ss::streets_clock_singleton::time_in_ms(), detected_object._timestamp ); + ss::streets_clock_singleton::sleep_for(abs(delay)); + } + // If delay is negative and service is not in simulation mode + // indicates sensor and service are not time sychronized. + else if( delay < 0 ) { + SPDLOG_WARN( + R"(Current time is {0}ms and detection time stamp is {1}ms. Sensor Data Sharing Service and sensor producing detections to not appear to be time synchronized)", + ss::streets_clock_singleton::time_in_ms(), + detected_object._timestamp ); + + } + + // Write Lock + std::unique_lock lock(detected_objects_lock); + detected_objects[detected_object._object_id] = detected_object; + SPDLOG_DEBUG("Detected Object List Size {0} after consumed: {1}", detected_objects.size(), payload); + + } + } + catch (const streets_utils::json_utils::json_parse_exception &e) { + SPDLOG_ERROR("Exception occured consuming detection message : {0}", e.what()); + } + } + SPDLOG_ERROR("Something went wrong, no longer consuming detections." ); + + } + + void sds_service::produce_sdsms() { + if ( !sdsm_producer ) { + throw std::runtime_error("SDSM consumer is null!"); + } + SPDLOG_INFO("Starting SDSM Producer!"); + while ( sdsm_producer->is_running() ) { + try{ + if ( !detected_objects.empty() ) { + streets_utils::messages::sdsm::sensor_data_sharing_msg msg = create_sdsm(); + const std::string json_msg = streets_utils::messages::sdsm::to_json(msg); + SPDLOG_DEBUG("Sending SDSM : {0}", json_msg); + sdsm_producer->send(json_msg); + // Message Count max is 127, reset after max value + if ( this->_message_count <= 127) { + this->_message_count++; + }else { + this->_message_count = 0; + } + // Write Lock + std::unique_lock lock(detected_objects_lock); + // Clear detected object + detected_objects.clear(); + } + } + catch( const streets_utils::json_utils::json_parse_exception &e) { + SPDLOG_ERROR("Exception occurred producing SDSM : {0}", e.what()); + } + ss::streets_clock_singleton::sleep_for(100); // Sleep for 100 ms between publish + } + SPDLOG_CRITICAL("SDSM Producers no longer running."); + + } + + streets_utils::messages::sdsm::sensor_data_sharing_msg sds_service::create_sdsm() { + streets_utils::messages::sdsm::sensor_data_sharing_msg msg; + // Read lock + uint64_t timestamp = ss::streets_clock_singleton::time_in_ms(); + msg._time_stamp = to_sdsm_timestamp(timestamp); + // Populate with rolling counter + msg._msg_count = this->_message_count; + // Populate with infrastructure id + msg._source_id = this->_infrastructure_id; + // Populate equipement type + msg._equipment_type = sdsm::equipment_type::RSU; + // Polulate ref position + msg._ref_positon = to_position_3d(this->sdsm_reference_point); + std::shared_lock lock(detected_objects_lock); + for (const auto &[object_id, object] : detected_objects){ + auto ned_object = detected_object_enu_to_ned(object); + auto detected_object_data = to_detected_object_data(ned_object,timestamp); + // TODO: Update time offset. Currently CARMA-Streets detected object message does not support timestamp + // This is a bug and needs to be addressed. + msg._objects.push_back(detected_object_data); + } + return msg; + } + + + void sds_service::start() { + SPDLOG_DEBUG("Starting Sensor Data Sharing Service"); + streets_service::start(); + std::thread detection_thread(&sds_service::consume_detections, this); + std::thread sdsm_thread(&sds_service::produce_sdsms, this); + detection_thread.join(); + sdsm_thread.join(); + } + + streets_utils::messages::sdsm::position_3d to_position_3d(const lanelet::GPSPoint &ref_position) { + streets_utils::messages::sdsm::position_3d position; + // Convert to 1/10 of microdegrees + position._longitude = static_cast(ref_position.lon * 1e7); + position._latitude = static_cast(ref_position.lat *1e7); + // Convert 0.1 meters + position._elevation = static_cast(ref_position.ele * 10); + return position; + + } +} \ No newline at end of file diff --git a/sensor_data_sharing_service/test/detected_object_enu_to_ned_converter_test.cpp b/sensor_data_sharing_service/test/detected_object_enu_to_ned_converter_test.cpp new file mode 100644 index 000000000..e2a22a959 --- /dev/null +++ b/sensor_data_sharing_service/test/detected_object_enu_to_ned_converter_test.cpp @@ -0,0 +1,45 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include +#include +namespace sensor_data_sharing_service { + TEST(DetectedObjectToENUToNEDConverterTest, TestConversion) { + streets_utils::messages::detected_objects_msg::detected_objects_msg enu_detection; + auto pos_x = 4.3; + auto pos_y = -6.3; + auto pos_z = 10; + auto vel_x = -3.2; + auto vel_y = 0.7; + auto vel_z = -5.0; + + enu_detection._position._x = pos_x; + enu_detection._position._y = pos_y; + enu_detection._position._z = pos_z; + enu_detection._velocity._x = vel_x; + enu_detection._velocity._y = vel_y; + enu_detection._velocity._z = vel_z; + + auto ned_detection = detected_object_enu_to_ned(enu_detection); + EXPECT_DOUBLE_EQ( ned_detection._position._x, pos_y); + EXPECT_DOUBLE_EQ( ned_detection._position._y, pos_x); + EXPECT_DOUBLE_EQ( ned_detection._position._z, -pos_z); + + EXPECT_DOUBLE_EQ( ned_detection._velocity._x, vel_y); + EXPECT_DOUBLE_EQ( ned_detection._velocity._y, vel_x); + EXPECT_DOUBLE_EQ( ned_detection._velocity._z, -vel_z); + + } +} \ No newline at end of file diff --git a/sensor_data_sharing_service/test/detected_object_to_sdsm_converter_test.cpp b/sensor_data_sharing_service/test/detected_object_to_sdsm_converter_test.cpp new file mode 100644 index 000000000..a0d81e818 --- /dev/null +++ b/sensor_data_sharing_service/test/detected_object_to_sdsm_converter_test.cpp @@ -0,0 +1,234 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include +#include +#include + +namespace sensor_data_sharing_service { + TEST(detectedObjectToSdsmConverterTest, testToObjectType){ + EXPECT_EQ(streets_utils::messages::sdsm::object_type::VEHICLE, to_object_type("CAR")); + EXPECT_EQ(streets_utils::messages::sdsm::object_type::VRU, to_object_type("CYCLIST")); + EXPECT_EQ(streets_utils::messages::sdsm::object_type::UNKNOWN, to_object_type("TREE")); + } + + TEST(detectedObjectToSdsmConverterTest, testToDetectedObjectData) { + // Create detected object + streets_utils::messages::detected_objects_msg::detected_objects_msg msg; + msg._object_id = 123; + msg._type = "TREE"; + msg._sensor_id = "sensor_1"; + msg._proj_string = "some string"; + msg._confidence = 0.9; + msg._velocity = {0.1, 2.3, 5.2}; + msg._angular_velocity = {0.1, 2.3, 5.2}; + msg._position = {0.1, 2.3, 5.2}; + msg._position_covariance = {{0.1, 2.3, 5.2},{0.1, 2.3, 5.2},{0.1, 2.3, 5.2}}; + msg._velocity_covariance = {{0.1, 2.3, 5.2},{0.1, 2.3, 5.2},{0.1, 2.3, 5.2}}; + msg._angular_velocity_covariance = {{0.1, 2.3, 5.2},{0.1, 2.3, 5.2},{0.1, 2.3, 5.2}}; + msg._size._length =1.1; + msg._size._width =1.1; + msg._size._height = 10; + msg._timestamp = 100; + + auto data = to_detected_object_data(msg, 200); + EXPECT_EQ(msg._object_id, data._detected_object_common_data._object_id); + EXPECT_EQ(streets_utils::messages::sdsm::object_type::UNKNOWN, data._detected_object_common_data._object_type); + EXPECT_EQ(90, data._detected_object_common_data._classification_confidence); + EXPECT_EQ(static_cast(msg._position._x*10), data._detected_object_common_data._position_offset._offset_x); + EXPECT_EQ(static_cast(msg._position._y*10), data._detected_object_common_data._position_offset._offset_y); + EXPECT_EQ(static_cast(msg._position._y*10), data._detected_object_common_data._position_offset._offset_y); + + EXPECT_EQ(115, data._detected_object_common_data._speed); + EXPECT_EQ(260, data._detected_object_common_data._speed_z); + EXPECT_EQ(static_cast(msg._size._length*10), std::get(data._detected_object_optional_data.value())._size._length); + EXPECT_EQ(100, data._detected_object_common_data._time_measurement_offset); + + } + + TEST(detectedObjectToSdsmConverterTest, testToDetectedObjectDataWithNegativeVelocity) { + // Create detected object + streets_utils::messages::detected_objects_msg::detected_objects_msg msg; + msg._object_id = 123; + msg._type = "TREE"; + msg._sensor_id = "sensor_1"; + msg._proj_string = "some string"; + msg._confidence = 0.9; + msg._velocity = {-0.1, -2.3, -5.2}; + msg._angular_velocity = {0.1, 2.3, 5.2}; + msg._position = {0.1, 2.3, 5.2}; + msg._position_covariance = {{0.1, 2.3, 5.2},{0.1, 2.3, 5.2},{0.1, 2.3, 5.2}}; + msg._velocity_covariance = {{0.1, 2.3, 5.2},{0.1, 2.3, 5.2},{0.1, 2.3, 5.2}}; + msg._angular_velocity_covariance = {{0.1, 2.3, 5.2},{0.1, 2.3, 5.2},{0.1, 2.3, 5.2}}; + msg._size._length =1.1; + msg._size._width =1.1; + msg._size._height = 10; + msg._timestamp = 200; + + auto data = to_detected_object_data(msg, 100); + EXPECT_EQ(msg._object_id, data._detected_object_common_data._object_id); + EXPECT_EQ(streets_utils::messages::sdsm::object_type::UNKNOWN, data._detected_object_common_data._object_type); + EXPECT_EQ(90, data._detected_object_common_data._classification_confidence); + EXPECT_EQ(static_cast(msg._position._x*10), data._detected_object_common_data._position_offset._offset_x); + EXPECT_EQ(static_cast(msg._position._y*10), data._detected_object_common_data._position_offset._offset_y); + EXPECT_EQ(static_cast(msg._position._y*10), data._detected_object_common_data._position_offset._offset_y); + EXPECT_EQ(115, data._detected_object_common_data._speed); + EXPECT_EQ(260, data._detected_object_common_data._speed_z); + EXPECT_EQ(static_cast(msg._size._length*10), std::get(data._detected_object_optional_data.value())._size._length); + + EXPECT_EQ(-100, data._detected_object_common_data._time_measurement_offset); + + } + + TEST(detectedObjectToSdsmConverterTest, toSdsmTimestampTest) { + // Time 2023-12-11T19:07:44.075Z + uint64_t epoch_timestamp = 1702321664075; + auto sdsm_timestamp = to_sdsm_timestamp(epoch_timestamp); + EXPECT_EQ(2023, sdsm_timestamp.year); + EXPECT_EQ(12, sdsm_timestamp.month); + EXPECT_EQ(11, sdsm_timestamp.day); + EXPECT_EQ(19, sdsm_timestamp.hour); + EXPECT_EQ(7, sdsm_timestamp.minute); + EXPECT_EQ(44075, sdsm_timestamp.second); + } + + TEST(detectedObjectToSdsmConverterTest, toPositionConfidenceSet) { + // Covariance matrix x variance 0.4m variance =0.4m and z variance 0.4m (diagonal) + std::vector> position_covariance = {{0.2, 2.3, 5.2},{0.1, 0.2, 5.2},{0.1, 2.3, 0.2}}; + auto position_confidence_set = to_position_confidence_set(position_covariance); + // 0.4m * 2 for 95 % confidence interval is closer to 1 m than to 50 cm + EXPECT_EQ( streets_utils::messages::sdsm::position_confidence::A_1M ,position_confidence_set._position_confidence ); + EXPECT_EQ( streets_utils::messages::sdsm::position_confidence::A_1M ,position_confidence_set._elevation_confidence ); + + } + + TEST(detectedObjectToSdsmConvertTest, toPositionConfidence) { + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_500M, to_position_confidence(550)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_500M, to_position_confidence(375)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_500M, to_position_confidence(350)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_200M, to_position_confidence(349)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_200M, to_position_confidence(150)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_100M, to_position_confidence(149)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_100M, to_position_confidence(75)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_50M, to_position_confidence(74)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_50M, to_position_confidence(35)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_20M, to_position_confidence(34)); + + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_500M, to_position_confidence(500)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_200M, to_position_confidence(200)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_100M, to_position_confidence(100)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_50M, to_position_confidence(50)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_20M, to_position_confidence(20)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_10M, to_position_confidence(10)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_5M, to_position_confidence(5)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_2M, to_position_confidence(2)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_1M, to_position_confidence(1)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_50CM, to_position_confidence(.50)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_20CM, to_position_confidence(.20)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_10CM, to_position_confidence(.10)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_5CM, to_position_confidence(.05)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_2CM, to_position_confidence(.02)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_1CM, to_position_confidence(.01)); + EXPECT_EQ(streets_utils::messages::sdsm::position_confidence::A_1CM, to_position_confidence(.001)); + + } + + + TEST(detectedObjectToSdsmConvertTest, toHeading) { + // to_heading converts velocity to heading (2 dimensional heading in units of 0.0125 degrees) + streets_utils::messages::detected_objects_msg::vector_3d velocity; + velocity._x = 0; + velocity._y= 0; + EXPECT_NEAR(0, to_heading(velocity),1); + velocity._x = 1; + velocity._y= 0; + EXPECT_NEAR(0, to_heading(velocity),1); + velocity._x = 1; + velocity._y = 1; + EXPECT_NEAR(3600, to_heading(velocity),1); + velocity._x = 0; + velocity._y = 1; + EXPECT_NEAR(7200, to_heading(velocity), 1); + velocity._x = -1; + velocity._y = 1; + EXPECT_NEAR(10800, to_heading(velocity),1); + velocity._x = -1; + velocity._y = 0; + EXPECT_NEAR(14400, to_heading(velocity),1); + velocity._x = -1; + velocity._y = -1; + EXPECT_NEAR(18000, to_heading(velocity),1); + velocity._x = 0; + velocity._y = -1; + EXPECT_NEAR(21600, to_heading(velocity),1); + velocity._x = 1; + velocity._y = -1; + EXPECT_NEAR(25200, to_heading(velocity),1); + + + } + + TEST(detectedObjectToSdsmConvertTest, toSpeed_XYZ_Confidence) { + std::vector> velocity_covariance = {{4, 2.3, 5.2},{0.1, 4, 5.2},{0.1, 2.3, 9}}; + auto speed_confidence = to_xy_speed_confidence(velocity_covariance); + auto speed_z_confidence = to_z_speed_confidence(velocity_covariance); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_5ms,speed_z_confidence); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_5ms,speed_confidence); + + } + + TEST(detectedObjectToSdsmConvertTest, toSpeedConfidence) { + + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_100ms,to_speed_confidence(100)); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_10ms,to_speed_confidence(10)); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_5ms,to_speed_confidence(5)); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_1ms,to_speed_confidence(1)); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_0_1ms,to_speed_confidence(0.1)); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_0_05ms,to_speed_confidence(0.05)); + EXPECT_EQ(streets_utils::messages::sdsm::speed_confidence::PREC_0_01ms,to_speed_confidence(0.01)); + + + } + + TEST(detectedObjectToSdsmConvertTest, toYawRateConfidence) { + std::vector> angular_velocity_covariance = {{4, 2.3, 5.2},{0.1, 4, 5.2},{0.1, 2.3, 9}}; + auto speed_confidence = to_yaw_rate_confidence(angular_velocity_covariance); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_05,speed_confidence); + angular_velocity_covariance = {{0.01, 0.0, 0.0},{0.0, 0.01, 0.0},{0.0, 0.0, 0.01}}; + speed_confidence = to_yaw_rate_confidence(angular_velocity_covariance); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_1,speed_confidence); + + } + + TEST(detectedObjectToSdsmConvertTest, toAngularVelocityConfidence) { + + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_100,to_angular_velocity_confidence(100)); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_10,to_angular_velocity_confidence(10)); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_05,to_angular_velocity_confidence(5)); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_01,to_angular_velocity_confidence(1)); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_1,to_angular_velocity_confidence(0.1)); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_05,to_angular_velocity_confidence(0.05)); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_01,to_angular_velocity_confidence(0.01)); + + } + + TEST(detectedObjectToSdsmConvertTest, toYawRate) { + EXPECT_NEAR(2939, to_yaw_rate(0.513), 1); + EXPECT_NEAR(-2939, to_yaw_rate(-0.513), 1); + + } + + +} \ No newline at end of file diff --git a/sensor_data_sharing_service/test/sensor_configuration_parser_test.cpp b/sensor_data_sharing_service/test/sensor_configuration_parser_test.cpp new file mode 100644 index 000000000..c30f107a8 --- /dev/null +++ b/sensor_data_sharing_service/test/sensor_configuration_parser_test.cpp @@ -0,0 +1,26 @@ +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include + + +namespace sensor_data_sharing_service { + TEST(sensor_configuration_parser_test, parse_sensor_location_test) { + auto pose = parse_sensor_location("/home/carma-streets/sensor_data_sharing_service/test/test_files/sensors.json", "sensor_1"); + EXPECT_NEAR(pose.x(), 1.0 , 0.01 ); + EXPECT_NEAR(pose.y(), 2.0 , 0.01 ); + EXPECT_NEAR(pose.z(), -3.2 , 0.01 ); + + } +} \ No newline at end of file diff --git a/sensor_data_sharing_service/test/sensor_data_sharing_service_test.cpp b/sensor_data_sharing_service/test/sensor_data_sharing_service_test.cpp new file mode 100644 index 000000000..0097b1dc6 --- /dev/null +++ b/sensor_data_sharing_service/test/sensor_data_sharing_service_test.cpp @@ -0,0 +1,189 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +using testing::_; +using testing::Return; +using testing::AnyNumber; +using testing::SaveArg; +using testing::DoAll; +namespace sensor_data_sharing_service { + + TEST(sensorDataSharingServiceTest, consumeDetections) { + // Set simulation mode to false + setenv(streets_service::SIMULATION_MODE_ENV.c_str(), "FALSE", 1); + sds_service serv; + + serv.initialize(); + // If consumer null expect runtime error + EXPECT_THROW(serv.consume_detections(), std::runtime_error); + serv.detection_consumer = std::make_shared(); + EXPECT_CALL(dynamic_cast(*serv.detection_consumer),subscribe()).Times(1); + EXPECT_CALL(dynamic_cast(*serv.detection_consumer),is_running()).Times(4) + .WillOnce(Return(true)) + .WillOnce(Return(true)) + .WillOnce(Return(true)) + .WillRepeatedly(Return(false)); + EXPECT_CALL(dynamic_cast(*serv.detection_consumer), consume(_)).Times(3).WillOnce(Return("")) + .WillOnce(Return("NOT JSON")) + .WillOnce(Return( + R"( + { + "type":"CAR", + "confidence":0.7, + "sensorId":"sensor1", + "projString":"projectionString2", + "objectId":123, + "position":{ + "x":-1.1, + "y":-2.0, + "z":-3.2 + }, + "positionCovariance":[[1.0,0.0,0.0],[1.0,0.0,0.0],[1.0,0.0,0.0]], + "velocity":{ + "x":1.0, + "y":1.0, + "z":1.0 + }, + "velocityCovariance":[[1.0,0.0,0.0],[1.0,0.0,0.0],[1.0,0.0,0.0]], + "angularVelocity":{ + "x":0.1, + "y":0.2, + "z":0.3 + }, + "angularVelocityCovariance":[[1.0,0.0,0.0],[1.0,0.0,0.0],[1.0,0.0,0.0]], + "size":{ + "length":2.0, + "height":1.0, + "width":0.5 + }, + "timestamp":1000 + } + )" + )); + + serv.consume_detections(); + // Consumed detection timestamp is more than 500 ms older than current real wall time. + // TODO: Create test case that covers condition for consuming up to date detection data. + EXPECT_EQ(serv.detected_objects.size(), 0); + } + + TEST(sensorDataSharingServiceTest, produceSdsms) { + + sds_service serv; + serv._infrastructure_id = "rsu_1234"; + // Initialize streets_clock in non simulation mode + streets_service::streets_clock_singleton::create(false); + // If producer null expect runtime error + + EXPECT_THROW(serv.produce_sdsms(), std::runtime_error); + const std::string detected_object_json = + R"( + { + "type":"TRUCK", + "confidence":1.0, + "sensorId":"IntersectionLidar", + "projString":"+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs", + "objectId":222, + "position":{ + "x":-23.70157158620998, + "y":-4.561758806718842, + "z":-9.991932254782586 + }, + "positionCovariance":[[0.04000000000000001,0.0,0.0],[0.0,0.04000000000000001,0.0],[0.0,0.0,0.04000000000000001]], + "velocity":{ + "x":1.0, + "y":0.0, + "z":0.0 + }, + "velocityCovariance":[[0.04000000000000001,0.0,0.0],[0.0,0.04000000000000001,0.0],[0.0,0.0,0.04000000000000001]], + "angularVelocity":{ + "x":0.0, + "y":0.0, + "z":0.0 + }, + "angularVelocityCovariance":[[0.010000000000000002,0.0,0.0],[0.0,0.010000000000000002,0.0],[0.0,0.0,0.010000000000000002]], + "size":{ + "length":2.601919174194336, + "height":1.3072861433029175, + "width":1.2337223291397095 + }, + "timestamp":41343 + } + )"; + auto detected_object = streets_utils::messages::detected_objects_msg::from_json(detected_object_json); + serv.detected_objects[detected_object._object_id] =detected_object; + serv.sdsm_producer = std::make_shared(); + EXPECT_CALL(dynamic_cast(*serv.sdsm_producer),is_running()).Times(3).WillOnce(Return(true)) + .WillOnce(Return(true)) + .WillRepeatedly(Return(false)); + std::string sdsm_json; + EXPECT_CALL(dynamic_cast(*serv.sdsm_producer), send(_)).WillOnce( DoAll(SaveArg<0>(&sdsm_json)) ); + serv.produce_sdsms(); + + // Verify produced json + SPDLOG_INFO("Produces SDMS {0}", sdsm_json); + streets_utils::messages::sdsm::sensor_data_sharing_msg msg = streets_utils::messages::sdsm::from_json(sdsm_json); + EXPECT_EQ( msg._equipment_type, streets_utils::messages::sdsm::equipment_type::RSU); + EXPECT_EQ( msg._source_id, serv._infrastructure_id ); + // http://en.cppreference.com/w/cpp/chrono/c/time + const std::time_t now = std::time(nullptr) ; // get the current time point + + // convert it to (local) calendar time + // http://en.cppreference.com/w/cpp/chrono/c/localtime + const std::tm calendar_time = *std::localtime( std::addressof(now) ) ; + EXPECT_EQ(calendar_time.tm_year+1900, msg._time_stamp.year); + EXPECT_EQ(calendar_time.tm_mon+1, msg._time_stamp.month); + EXPECT_EQ(calendar_time.tm_mday, msg._time_stamp.day); + EXPECT_EQ(calendar_time.tm_hour, msg._time_stamp.hour); + // Account for runs near close of minute + EXPECT_NEAR(calendar_time.tm_min, msg._time_stamp.minute, 1); + EXPECT_EQ(1 , msg._objects.size()); + EXPECT_EQ(streets_utils::messages::sdsm::object_type::VEHICLE, msg._objects[0]._detected_object_common_data._object_type); + EXPECT_TRUE(msg._objects[0]._detected_object_common_data._yaw_rate_confidence.has_value()); + EXPECT_EQ(streets_utils::messages::sdsm::angular_velocity_confidence::DEGSEC_0_1 , msg._objects[0]._detected_object_common_data._yaw_rate_confidence); + EXPECT_EQ(serv.detected_objects.size(), 0); + // SDSM assumes NED coordinate frame. Incoming detection is ENU. 1,0 in ENU is 0,1 in NED and is a 90 degree heading (heading is calculated from velocity) + EXPECT_NEAR( msg._objects[0]._detected_object_common_data._heading, 7200, 2); + } + + TEST(sensorDataSharingServiceTest,readLanelet2Map) { + sds_service serv; + EXPECT_TRUE(serv.read_lanelet_map("/home/carma-streets/sample_map/town01_vector_map_test.osm")); + EXPECT_NE(nullptr, serv.map_ptr); + } + + TEST(sensorDataSharingServiceTest, toPosition3d) { + lanelet::GPSPoint point{38.9551605829,-77.14701567,1}; + streets_utils::messages::sdsm::position_3d position = to_position_3d(point); + EXPECT_EQ(389551605, position._latitude ); + EXPECT_EQ(-771470156, position._longitude); + EXPECT_EQ(10, position._elevation); + } + + +} \ No newline at end of file diff --git a/sensor_data_sharing_service/test/test_files/sensors.json b/sensor_data_sharing_service/test/test_files/sensors.json new file mode 100644 index 000000000..162728b45 --- /dev/null +++ b/sensor_data_sharing_service/test/test_files/sensors.json @@ -0,0 +1,16 @@ +[ + { + "sensorId": "sensor_1", + "type": "SemanticLidar", + "location": { + "x": 1.0, + "y": 2.0, + "z": -3.2 + }, + "orientation": { + "yaw": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + } +] \ No newline at end of file diff --git a/signal_opt_service/CMakeLists.txt b/signal_opt_service/CMakeLists.txt index a76ee77c3..5a5229320 100644 --- a/signal_opt_service/CMakeLists.txt +++ b/signal_opt_service/CMakeLists.txt @@ -71,12 +71,11 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_lib ) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") set(BINARY ${PROJECT_NAME}_test) file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) -set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) add_executable(${BINARY} ${TEST_SOURCES}) add_test(NAME ${BINARY} COMMAND ${BINARY}) target_include_directories(${BINARY} PUBLIC ${PROJECT_SOURCE_DIR}/include) target_link_libraries(${BINARY} - PUBLIC + PRIVATE ${PROJECT_NAME}_lib spdlog::spdlog rapidjson diff --git a/signal_opt_service/Dockerfile b/signal_opt_service/Dockerfile index 166b4ab82..3d379fec3 100644 --- a/signal_opt_service/Dockerfile +++ b/signal_opt_service/Dockerfile @@ -12,6 +12,7 @@ RUN echo " ------> Install rapidjson..." WORKDIR /home/carma-streets/ext RUN git clone https://github.com/Tencent/rapidjson WORKDIR /home/carma-streets/ext/rapidjson/ +RUN git checkout a95e013b97ca6523f32da23f5095fcc9dd6067e5 RUN mkdir build WORKDIR /home/carma-streets/ext/rapidjson/build RUN cmake .. && make -j @@ -20,7 +21,7 @@ RUN make install # Install librdkafka RUN echo " ------> Install librdkafka..." WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/confluentinc/librdkafka.git +RUN git clone https://github.com/confluentinc/librdkafka.git -b v2.2.0 WORKDIR /home/carma-streets/ext/librdkafka/ RUN cmake -H. -B_cmake_build RUN cmake --build _cmake_build diff --git a/signal_opt_service/test/test_signal_opt_processing_worker.cpp b/signal_opt_service/test/test_signal_opt_processing_worker.cpp index f7b96de78..314b0232e 100644 --- a/signal_opt_service/test/test_signal_opt_processing_worker.cpp +++ b/signal_opt_service/test/test_signal_opt_processing_worker.cpp @@ -19,6 +19,8 @@ namespace signal_opt_service protected: void SetUp() override { + streets_service::streets_configuration::create("/home/carma-streets/message_services/manifest.json"); + streets_service::streets_clock_singleton::create(false); tsc_state = std::make_shared(); std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::milliseconds epochMs = std::chrono::duration_cast(now.time_since_epoch()); @@ -247,6 +249,7 @@ namespace signal_opt_service TEST_F(test_signal_opt_processing_worker, select_optimal_dpp) { + auto so_processing_worker = std::make_shared(); std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::milliseconds epochMs = std::chrono::duration_cast(now.time_since_epoch()); diff --git a/streets_service_base/Dockerfile b/streets_service_base/Dockerfile new file mode 100644 index 000000000..5741fbd74 --- /dev/null +++ b/streets_service_base/Dockerfile @@ -0,0 +1,4 @@ +ARG UBUNTU_CODENAME +FROM ghcr.io/usdot-fhwa-stol/carma-builds-x64:carma-system-4.5.0-${UBUNTU_CODENAME} +COPY ./build_scripts /opt/carma-streets/build_scripts +RUN /opt/carma-streets/build_scripts/install_streets_service_base_dependencies.sh diff --git a/streets_service_base/README.md b/streets_service_base/README.md new file mode 100644 index 000000000..5659906da --- /dev/null +++ b/streets_service_base/README.md @@ -0,0 +1,16 @@ +# Streets Service Base +## Introduction +This base image is intended for any basic **CARMA Streets** service. The **streets service base** image installs our logging library [spdlog](https://github.com/gabime/spdlog), kafka client library [librdkafka](https://github.com/confluentinc/librdkafka), and our JSON parsing/serializing library (rapidjson)[https://miloyip.github.io/rapidjson/] on top of the `carma-builds-x64` image to provide common CARMA Streets dependencies.The `carma-builds-x64` image is built from the (carma-builds)[https://github.com/usdot-fhwa-stol/carma-builds] github repository. This repository creates images in different ubuntu distributions and CPU architectures that function as build environments for C++ applications. Additionally, the **streets serivce base** image adds our debian package repository `http://s3.amazonaws.com/stol-apt-repository` to apt and installs (carma-time-lib)[https://github.com/usdot-fhwa-stol/carma-time-lib], the library we use to allow our CARMA Streets services to function both in simulation and real-world. This base image should be used for any CARMA Streets services. + +> [!IMPORTANT]\ +> Currently this base image is only being used for the **Sensor Data Sharing Service** but the **Signal Optimization Service**, **Scheduling Service** and the **TSC Service** should all be updated to build off this base image. + +## Usage +Simply use the following to extend this image +``` +FROM usdotfhwastoldev/streets_service_base: +``` +To build this image, use a docker build command similar to the one below +``` +docker build -t usdotfhwastoldev/streets_service_base: -f streets_service_base/Dockerfile --build-arg=UBUNTU_CODENAME= . +``` \ No newline at end of file diff --git a/streets_service_base_lanelet_aware/Dockerfile b/streets_service_base_lanelet_aware/Dockerfile new file mode 100644 index 000000000..02780d06f --- /dev/null +++ b/streets_service_base_lanelet_aware/Dockerfile @@ -0,0 +1,3 @@ +FROM usdotfhwastol/streets_service_base:carma-system-4.5.0-bionic +ENV LANELET2_MAP="/home/carma-streets/MAP/Intersection.osm" +RUN /opt/carma-streets/build_scripts/install_lanelet2_dependencies.sh diff --git a/streets_service_base_lanelet_aware/README.md b/streets_service_base_lanelet_aware/README.md new file mode 100644 index 000000000..2ef7cdd51 --- /dev/null +++ b/streets_service_base_lanelet_aware/README.md @@ -0,0 +1,18 @@ +# Streets Service Base Lanelet Aware +## Introduction +This base image is intented for **CARMA Streets** services that require [lanelet2](https://github.com/fzi-forschungszentrum-informatik/Lanelet2) for spatial understanding of the surrouding area. Currently both **Message Service** and **Intersection Model** both depend on [lanelet2](https://github.com/fzi-forschungszentrum-informatik/Lanelet2) for this purpose. The new propose **Sensor Data Sharing Service** will also need lanelet2 for map transforms and for eventual perception. To allow all three services and any future services easy access to this dependency, this new **Streets Service Base Lanelet Aware** image exist. +> [!IMPORTANT]\ +> Currently this base image is only being used for the **Sensor Data Sharing Service** but both the **Intersection_Model** and **Message Service** should be ported to this base image as well. + +## Usage +Simply use the following to extend this image +``` +FROM usdotfhwastoldev/streets_service_base_lanelet_aware: +``` +The image also contains the environment variable **LANELET2_MAP**, which is assigned a default value of `/home/carma-streets/MAP/Intersection.osm`, but can be overwritten using the docker-compose environment variable section of the service description. An example is shown belo +``` + lanlet_aware_service: + image: usdotfhwastoldev/lanlet_aware_service:develop + environment: + LANELET2_MAP: /new/path/to/lanelet2/map.osm +``` \ No newline at end of file diff --git a/streets_utils/json_utils/CMakeLists.txt b/streets_utils/json_utils/CMakeLists.txt new file mode 100644 index 000000000..8db72efef --- /dev/null +++ b/streets_utils/json_utils/CMakeLists.txt @@ -0,0 +1,84 @@ +# Copyright 2019-2023 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +cmake_minimum_required(VERSION 3.10.2) +project(json_utils) +# Set Flags +set(CMAKE_CXX_STANDARD 17) +# GNU standard installation directories +include(GNUInstallDirs) +# Find Packages +find_package(RapidJSON REQUIRED) +find_package(GTest REQUIRED) +find_package(spdlog REQUIRED) + +# Add definition for rapidjson to include std::string +add_definitions(-DRAPIDJSON_HAS_STDSTRING=1) +set(LIBRARY_NAME ${PROJECT_NAME}_lib) +######################################################## +# Build Library +######################################################## +file(GLOB JSON_UTILS_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) +add_library(${LIBRARY_NAME} ${JSON_UTILS_SOURCES} ) +target_include_directories(${LIBRARY_NAME} + PUBLIC + $) +target_link_libraries( ${LIBRARY_NAME} + PUBLIC + rapidjson + spdlog::spdlog +) + +######################################################## +# Install streets_utils::json_utils_lib library. +######################################################## + +install( + TARGETS ${LIBRARY_NAME} + EXPORT ${LIBRARY_NAME}Targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/streets_utils/ + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/streets_utils/${LIBRARY_NAME} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/streets_utils/ +) +install( + EXPORT ${LIBRARY_NAME}Targets + FILE ${LIBRARY_NAME}Targets.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${LIBRARY_NAME} + NAMESPACE streets_utils:: +) +include(CMakePackageConfigHelpers) +configure_package_config_file( + cmake/${LIBRARY_NAME}Config.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${LIBRARY_NAME}) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${LIBRARY_NAME} +) + +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/streets_utils/${LIBRARY_NAME} + FILES_MATCHING PATTERN "*.hpp" # select header files + ) +######################## +# googletest for unit testing +######################## +set(TEST_NAME ${PROJECT_NAME}_test) +file(GLOB_RECURSE JSON_UTILS_TEST_SOURCES LIST_DIRECTORIES false test/*.hpp test/*.cpp) +add_executable(${TEST_NAME} ${JSON_UTILS_TEST_SOURCES}) +gtest_discover_tests(${TEST_NAME}) +target_link_libraries(${TEST_NAME} + PRIVATE + ${LIBRARY_NAME} + GTest::Main + ) \ No newline at end of file diff --git a/streets_utils/json_utils/README.md b/streets_utils/json_utils/README.md new file mode 100644 index 000000000..7036d7d1a --- /dev/null +++ b/streets_utils/json_utils/README.md @@ -0,0 +1,28 @@ +# JSON Utility Library + +## Introduction + +This CARMA-Streets library contains utility functions for parsing JSON strings with [RapidJSON](https://miloyip.github.io/rapidjson/index.html). The functions exposed by this library are described below + +## Functions +`rapidjson::Document parse_json( const std::string &json )` + +Function to parse `std::string` JSON with [RapidJSON](https://miloyip.github.io/rapidjson/index.html) with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Throws `json_utils_exception` if string is not valid json. Otherwise returns `rapidjson::Document`` + +`std::optional parse_int_member(const std::string &member_name, const rapidjson::Value &doc, bool required )` +`std::optional parse_uint_member(const std::string &member_name, const rapidjson::Value &doc, bool required )` +`std::optional parse_bool_member(const std::string &member_name, const rapidjson::Value &doc, bool required )` +`std::optional parse_string_member(const std::string &member_name, const rapidjson::Value &doc, bool required )` +`std::optional parse_double_member(const std::string &member_name, const rapidjson::Value &doc, bool required)` +`std::optional parse_object_member(const std::string &member_name, const rapidjson::Value &doc, bool required )` +`std::optional parse_array_member(const std::string &member_name, const rapidjson::Value &doc, bool required)` + +Functions to retrieve member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized `std::optional` for optional members that are not found in JSON object and will throw `json_utils_exception` for required members that are not found in JSON object. + +## Include Library +To include after installing with `make install` simply add find_package call to `CMakeLists.txt` and link the library as follows. Installed CMake configuration files should handle finding and linking depedencies `RapidJSON` and `spdlog`. The library along with it's dependencies can then be included by simply using the find_package() instruction. +``` +find_package(json_utils_lib) +... +target_link_library( target PUBLIC streets_utils::json_utils) +``` \ No newline at end of file diff --git a/streets_utils/json_utils/cmake/json_utils_libConfig.cmake.in b/streets_utils/json_utils/cmake/json_utils_libConfig.cmake.in new file mode 100644 index 000000000..6b665ab53 --- /dev/null +++ b/streets_utils/json_utils/cmake/json_utils_libConfig.cmake.in @@ -0,0 +1,7 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(spdlog REQUIRED) +find_dependency(RapidJSON REQUIRED) + +include("${CMAKE_CURRENT_LIST_DIR}/json_utils_libTargets.cmake") diff --git a/streets_utils/json_utils/include/json_document_parse_error.hpp b/streets_utils/json_utils/include/json_document_parse_error.hpp new file mode 100644 index 000000000..b92430193 --- /dev/null +++ b/streets_utils/json_utils/include/json_document_parse_error.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +namespace streets_utils::json_utils { + class json_document_parse_error : public std::runtime_error { + public: + explicit json_document_parse_error(const std::string &msg, const rapidjson::Document &doc ) : + std::runtime_error(msg + "\nError :" + std::to_string(doc.GetParseError()) + " Offset: " + std::to_string(doc.GetErrorOffset())){}; + + }; +} \ No newline at end of file diff --git a/streets_utils/json_utils/include/json_utils.hpp b/streets_utils/json_utils/include/json_utils.hpp new file mode 100644 index 000000000..7eb4c64bd --- /dev/null +++ b/streets_utils/json_utils/include/json_utils.hpp @@ -0,0 +1,122 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "json_utils_exception.hpp" +#include "json_document_parse_error.hpp" + +namespace streets_utils::json_utils { + + /** + * @brief Function to parse `std::string` JSON with [RapidJSON](https://miloyip.github.io/rapidjson/index.html) with + * [DOM parsing](https://miloyip.github.io/rapidjson/md_obj_dom.html). Throws `json_parse_exception` if string is + * not valid json. Otherwise returns `rapidjson::Document` + * @param json std::string JSON to parse + * @throws json_parse_exception if passed string is not valid json. + * @return resulting `rapidjson::Document` + */ + rapidjson::Document parse_json( const std::string &json ); +/** + * @brief Function to parse file containing JSON with [RapidJSON](https://miloyip.github.io/rapidjson/index.html) with + * [DOM parsing](https://miloyip.github.io/rapidjson/md_obj_dom.html). Throws `json_parse_exception` if string is + * not valid json. Otherwise returns `rapidjson::Document` + * @param json std::string JSON to parse + * @throws json_parse_exception if passed string is not valid json. + * @return resulting `rapidjson::Document` + */ + rapidjson::Document parse_json_file(const std::string &filepath); + /** + * @brief Functions to retrieve int member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) + * with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized + * `std::optional` for optional members that are not found in JSON object and will throw `json_parse_exception`. + * for required members that are not found in JSON object. + * @param member_name string member name to search for. + * @param obj JSON object with member + * @param required bool flag to indicate whether member is required by calling code. + * @throws json_parse_exception if member is required but not found. + * @return std::optional + */ + std::optional parse_int_member(const std::string &member_name, const rapidjson::Value &obj, bool required ); + /** + * @brief Functions to retrieve uint member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) + * with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized + * `std::optional` for optional members that are not found in JSON object and will throw `json_parse_exception`. + * for required members that are not found in JSON object. + * @param member_name string member name to search for. + * @param obj JSON object with member + * @param required bool flag to indicate whether member is required by calling code. + * @throws json_parse_exception if member is required but not found. + * @return std::optional + */ + std::optional parse_uint_member(const std::string &member_name, const rapidjson::Value &obj, bool required ); + /** + * @brief Functions to retrieve bool member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) + * with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized + * `std::optional` for optional members that are not found in JSON object and will throw `json_parse_exception`. + * for required members that are not found in JSON object. + * @param member_name string member name to search for. + * @param obj JSON object with member + * @param required bool flag to indicate whether member is required by calling code. + * @throws json_parse_exception if member is required but not found. + * @return std::optional + */ + std::optional parse_bool_member(const std::string &member_name, const rapidjson::Value &obj, bool required ); + /** + * @brief Functions to retrieve std::string member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) + * with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized + * `std::optional` for optional members that are not found in JSON object and will throw `json_parse_exception`. + * for required members that are not found in JSON object. + * @param member_name string member name to search for. + * @param obj JSON object with member + * @param required bool flag to indicate whether member is required by calling code. + * @throws json_parse_exception if member is required but not found. + * @return std::optional + */ + std::optional parse_string_member(const std::string &member_name, const rapidjson::Value &obj, bool required ); + /** + * @brief Functions to retrieve double member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) + * with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized + * `std::optional` for optional members that are not found in JSON object and will throw `json_parse_exception`. + * for required members that are not found in JSON object. + * @param member_name string member name to search for. + * @param obj JSON object with member + * @param required bool flag to indicate whether member is required by calling code. + * @throws json_parse_exception if member is required but not found. + * @return std::optional + */ + std::optional parse_double_member(const std::string &member_name, const rapidjson::Value &obj, bool required); + /** + * @brief Functions to retrieve object member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) + * with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized + * `std::optional` for optional members that are not found in JSON object and will throw `json_parse_exception`. + * for required members that are not found in JSON object. + * @param member_name string member name to search for. + * @param obj JSON object with member + * @param required bool flag to indicate whether member is required by calling code. + * @throws json_parse_exception if member is required but not found. + * @return std::optional + */ + std::optional parse_object_member(const std::string &member_name, const rapidjson::Value &obj, bool required ); + /** + * @brief Functions to retrieve array member values from JSON object [RapidJSON](https://miloyip.github.io/rapidjson/index.html) + * with [DOM parsing](https://miloyip.github.io/rapidjson/md_doc_dom.html). Functions will return unintialized + * `std::optional` for optional members that are not found in JSON object and will throw `json_parse_exception`. + * for required members that are not found in JSON object. + * @param member_name string member name to search for. + * @param obj JSON object with member + * @param required bool flag to indicate whether member is required by calling code. + * @throws json_parse_exception if member is required but not found. + * @return std::optional + */ + std::optional parse_array_member(const std::string &member_name, const rapidjson::Value &obj, bool required); + + + +} \ No newline at end of file diff --git a/streets_utils/json_utils/include/json_utils_exception.hpp b/streets_utils/json_utils/include/json_utils_exception.hpp new file mode 100644 index 000000000..b1c611d4b --- /dev/null +++ b/streets_utils/json_utils/include/json_utils_exception.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include + + + +namespace streets_utils::json_utils { + /** + * @brief Runtime exception related to json_utils functions. Thrown when : + * - Passing Invalid JSON + * - Missing Required member + * - Passing rapidjson::Value that is not an object into parse functions + * + * @author Paul Bourelly + */ + class json_parse_exception : public std::runtime_error{ + using std::runtime_error::runtime_error; + }; +} \ No newline at end of file diff --git a/streets_utils/json_utils/src/json_utils.cpp b/streets_utils/json_utils/src/json_utils.cpp new file mode 100644 index 000000000..d9a45ad20 --- /dev/null +++ b/streets_utils/json_utils/src/json_utils.cpp @@ -0,0 +1,123 @@ +#include "json_utils.hpp" + +namespace streets_utils::json_utils { + + rapidjson::Document parse_json(const std::string &json) { + rapidjson::Document obj; + obj.Parse(json); + if (obj.HasParseError()) + { + // TODO: Change to json_document_parse_exception. Requires changes to services and unit tests + throw json_parse_exception("Message JSON is misformatted. JSON parsing failed!"); + } + return obj; + } + + rapidjson::Document parse_json_file(const std::string &filepath) { + // Parse JSON configuration file + std::ifstream file(filepath); + if (!file.is_open()) { + throw std::invalid_argument("Unable to open Streets configuration file " + filepath + " due to : " + strerror(errno) + "."); + } + // Add file contents to stream and parse stream into Document + rapidjson::IStreamWrapper isw(file); + rapidjson::Document doc; + doc.ParseStream(isw); + if (doc.HasParseError()){ + throw json_document_parse_error("Encounter document parse error while attempting to parse sensor configuration file " + filepath + "!", doc); + } + file.close(); + return doc; + } + std::optional parse_int_member(const std::string &member_name, const rapidjson::Value &obj, bool required ){ + std::optional member; + if (obj.HasMember(member_name.c_str()) && obj.FindMember(member_name.c_str())->value.IsInt64()) + { + member = obj[member_name.c_str()].GetInt64(); + } + else if (required) + { + throw json_parse_exception("Missing or incorrect type for required member " + member_name + "!"); + } + return member; + } + + std::optional parse_uint_member(const std::string &member_name, const rapidjson::Value &obj, bool required ) { + std::optional member; + if (obj.HasMember(member_name.c_str()) && obj.FindMember(member_name.c_str())->value.IsUint64()) + { + member = obj[member_name.c_str()].GetUint64(); + } + else if (required) + { + throw json_parse_exception("Missing or incorrect type for required member " + member_name + "!"); + } + return member; + }; + + std::optional parse_bool_member(const std::string &member_name, const rapidjson::Value &obj, bool required ) { + std::optional member; + if (obj.HasMember(member_name.c_str()) && obj.FindMember(member_name.c_str())->value.IsBool()) + { + member = obj[member_name.c_str()].GetBool(); + } + else if (required) + { + throw json_parse_exception("Missing or incorrect type for required member " + member_name + "!"); + } + return member; + }; + + std::optional parse_string_member(const std::string &member_name, const rapidjson::Value &obj, bool required ){ + std::optional member; + if (obj.HasMember(member_name.c_str()) && obj.FindMember(member_name.c_str())->value.IsString()) + { + member = obj[member_name.c_str()].GetString(); + } + else if (required) + { + throw json_parse_exception("Missing or incorrect type for required member " + member_name + "!"); + } + return member; + }; + + std::optional parse_double_member(const std::string &member_name, const rapidjson::Value &obj, bool required) { + std::optional member; + if (obj.HasMember(member_name.c_str()) && obj.FindMember(member_name.c_str())->value.IsDouble()) + { + member = obj[member_name.c_str()].GetDouble(); + } + else if (required) + { + throw json_parse_exception("Missing or incorrect type for required member " + member_name + "!"); + } + return member; + }; + + std::optional parse_object_member(const std::string &member_name, const rapidjson::Value &obj, bool required) { + if (obj.HasMember(member_name.c_str()) && obj.FindMember(member_name.c_str())->value.IsObject()) + { + return obj[member_name.c_str()].GetObject(); + } + else if (required) + { + throw json_parse_exception("Missing or incorrect type for required member " + member_name + "!"); + } + return std::nullopt; + } + + std::optional parse_array_member(const std::string &member_name, const rapidjson::Value &obj, bool required) { + if (obj.HasMember(member_name.c_str()) && obj.FindMember(member_name.c_str())->value.IsArray()) + { + return obj[member_name.c_str()].GetArray(); + } + else if (required) + { + throw json_parse_exception("Missing or incorrect type for required member " + member_name + "!"); + } + return std::nullopt; + } + + + +} \ No newline at end of file diff --git a/streets_utils/json_utils/test/json_utils_test.cpp b/streets_utils/json_utils/test/json_utils_test.cpp new file mode 100644 index 000000000..9842f95e2 --- /dev/null +++ b/streets_utils/json_utils/test/json_utils_test.cpp @@ -0,0 +1,293 @@ +#include +#include +#include + +using namespace streets_utils::json_utils; + +//---------------------test parse_json--------------------- +TEST(JsonUtilsTest, testParseJsonInvalidJson) { + // Empty String + std::string invalid_json = ""; + EXPECT_THROW( parse_json(invalid_json), json_parse_exception); + + // Property missing quotations + invalid_json = "{ some_propert: \"some_value\"}"; + EXPECT_THROW( parse_json(invalid_json), json_parse_exception); + +} + +TEST(JsonUtilsTest, testParseJsonValidJson) { + // Correct JSON + std::string valid_json = "{ \"some_property\": \"some_value\"}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_FALSE(parsed_doc.HasParseError()); +} + +TEST(JsonUtilsTest, testParseJsonValidJsonFile) { + // Correct JSON + std::string file_path = "/home/carma-streets/streets_utils/json_utils/test/test_files/valid.json"; + auto parsed_doc = parse_json_file(file_path); + EXPECT_FALSE(parsed_doc.HasParseError()); + +} + +TEST(JsonUtilsTest, testParseJsonInvalidJsonFilePath) { + // Correct JSON + std::string file_path = "/home/invalid/filepath.json"; + EXPECT_THROW(parse_json_file(file_path), std::invalid_argument); +} + +TEST(JsonUtilsTest, testParseJsonInvalidJsonFile) { + // Correct JSON + std::string file_path = "/home/carma-streets/streets_utils/json_utils/test/test_files/invalid.json"; + EXPECT_THROW(parse_json_file(file_path), json_document_parse_error); +} + +//---------------------test parse_uint_member--------------------- +TEST(JsonUtilsTest, testGetJsonUintRequiredPropertyPresent){ + // Test with required property present + std::string valid_json = "{ \"some_property\": 12345}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_EQ( 12345, parse_uint_member("some_property", parsed_doc, true)); +} + +TEST(JsonUtilsTest, testGetJsonUintRequiredPropertyNotPresent){ + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_uint_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonUintRequiredPropertyWrongType){ + // Test with required property present with wrong type + std::string valid_json = "{ \"some_property\": -12345}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_uint_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonUintOptionalPropertyMissing) { + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + auto property = parse_uint_member("some_property", parsed_doc, false); + EXPECT_FALSE( property.has_value()); +} +//---------------------test get_json_int_property--------------------- + +TEST(JsonUtilsTest, testGetJsonIntRequiredPropertyPresent){ + // Test with required property present + std::string valid_json = "{ \"some_property\": -12345}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_EQ( -12345, parse_int_member("some_property", parsed_doc, true)); +} + +TEST(JsonUtilsTest, testGetJsonIntRequiredPropertyNotPresent){ + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_int_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonIntRequiredPropertyWrongType){ + // Test with required property present with wrong type + std::string valid_json = "{ \"some_property\": true}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_int_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonIntOptionalPropertyMissing) { + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + auto property = parse_int_member("some_property", parsed_doc, false); + EXPECT_FALSE( property.has_value()); +} +//---------------------test parse_bool_property--------------------- + +TEST(JsonUtilsTest, testGetJsonBoolRequiredPropertyPresent){ + // Test with required property present + std::string valid_json = "{ \"some_property\": true}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_EQ( true, parse_bool_member("some_property", parsed_doc, true)); +} + +TEST(JsonUtilsTest, testGetJsonBoolRequiredPropertyNotPresent){ + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": true}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_bool_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonBoolRequiredPropertyWrongType){ + // Test with required property present with wrong type + std::string valid_json = "{ \"some_property\": 1234}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_bool_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonBoolOptionalPropertyMissing) { + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + auto property = parse_bool_member("some_property", parsed_doc, false); + EXPECT_FALSE( property.has_value()); +} + +//---------------------test get_json_double_property--------------------- + +TEST(JsonUtilsTest, testGetJsonDoubleRequiredPropertyPresent){ + // Test with required property present + std::string valid_json = "{ \"some_property\": 12.3}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_NEAR( 12.3, parse_double_member("some_property", parsed_doc, true).value(), 0.01); +} + +TEST(JsonUtilsTest, testGetJsonDoubleRequiredPropertyNotPresent){ + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12.3}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_double_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonDoubleRequiredPropertyWrongType){ + // Test with required property present with wrong type + std::string valid_json = "{ \"some_property\": 1234}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_double_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonDoubleOptionalPropertyMissing) { + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + auto property = parse_double_member("some_property", parsed_doc, false); + EXPECT_FALSE( property.has_value()); +} +//---------------------test parse_string_property--------------------- + +TEST(JsonUtilsTest, testGetJsonStringRequiredPropertyPresent){ + // Test with required property present + std::string valid_json = "{ \"some_property\": \"some_property\" }"; + auto parsed_doc = parse_json(valid_json); + EXPECT_EQ("some_property", parse_string_member("some_property", parsed_doc, true)); +} + +TEST(JsonUtilsTest, testGetJsonStringRequiredPropertyNotPresent){ + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12.3}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_string_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonStringRequiredPropertyWrongType){ + // Test with required property present with wrong type + std::string valid_json = "{ \"some_property\": 1234}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_string_member("some_property", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonStringOptionalPropertyMissing) { + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + auto property = parse_string_member("some_property", parsed_doc, false); + EXPECT_FALSE( property.has_value()); +} + +//---------------------test parse_object_property--------------------- + +TEST(JsonUtilsTest, testGetJsonObjectRequiredPropertyPresent){ + // Test with required property present + std::string valid_json = "{ " + "\"some_object\": {" + "\"object_name\" : \"object\"," + "\"object_value\" : 123" + "}" + "}"; + auto parsed_doc = parse_json(valid_json); + auto object = parse_object_member("some_object", parsed_doc, true); + EXPECT_EQ("object", parse_string_member("object_name", object.value(), true)); + EXPECT_EQ( 123, parse_int_member("object_value", object.value(), true) ); + +} + +TEST(JsonUtilsTest, testGetJsonObjectRequiredPropertyNotPresent){ + // Test with required property no present + std::string valid_json = "{ " + "\"some_other_object\": {" + "\"object_name\" : \"object\"," + "\"object_value\" : 123" + "}" + "}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_object_member("some_object", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonObjectRequiredPropertyWrongType){ + // Test with required property present with wrong type + std::string valid_json = "{ " + "\"some_object\": [{" + "\"object_name\" : \"object\"," + "\"object_value\" : 123" + "}]" + "}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_object_member("some_object", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonObjectOptionalPropertyMissing) { + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + auto property = parse_object_member("some_property", parsed_doc, false); + EXPECT_FALSE( property.has_value()); +} +//---------------------test parse_array_property--------------------- + +TEST(JsonUtilsTest, testGetJsonArrayRequiredPropertyPresent){ + // Test with required property present + std::string valid_json = "{ " + "\"some_array\": " + "[456, 2452, -1232, 2345]" + "}"; + auto parsed_doc = parse_json(valid_json); + auto array = parse_array_member("some_array", parsed_doc, true); + + EXPECT_EQ(4, array.value().Size()); + EXPECT_EQ(456, array.value()[0].GetInt()); + EXPECT_EQ(2452, array.value()[1].GetInt()); + EXPECT_EQ(-1232, array.value()[2].GetInt()); + EXPECT_EQ(2345, array.value()[3].GetInt()); +} + +TEST(JsonUtilsTest, testGetJsonArrayRequiredPropertyNotPresent){ + // Test with required property no present + std::string valid_json = "{ " + "\"some_other_array\": " + "[456, 2452, -1232, 2345]" + "}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_array_member("some_array", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonArrayRequiredPropertyWrongType){ + // Test with required property present with wrong type + std::string valid_json = "{ " + "\"some_array\": [{" + "\"object_name\" : \"object\"," + "\"object_value\" : 123" + "}]" + "}"; + auto parsed_doc = parse_json(valid_json); + EXPECT_THROW( parse_array_member("some_object", parsed_doc, true), json_parse_exception); +} + +TEST(JsonUtilsTest, testGetJsonArrayOptionalPropertyMissing) { + // Test with required property no present + std::string valid_json = "{ \"some_property_other\": 12345}"; + auto parsed_doc = parse_json(valid_json); + auto property = parse_array_member("some_property", parsed_doc, false); + EXPECT_FALSE( property.has_value()); +} + + diff --git a/streets_utils/json_utils/test/test_files/invalid.json b/streets_utils/json_utils/test/test_files/invalid.json new file mode 100644 index 000000000..4b1721cba --- /dev/null +++ b/streets_utils/json_utils/test/test_files/invalid.json @@ -0,0 +1,16 @@ +[ + { + "sensorId": "sensor_1", + "type": "SemanticLidar",, + "location": { + "x": 1.0, + "y": 2.0, + "z": -3.2 + }, + "orientation": { + "yaw": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + } +] \ No newline at end of file diff --git a/streets_utils/json_utils/test/test_files/valid.json b/streets_utils/json_utils/test/test_files/valid.json new file mode 100644 index 000000000..162728b45 --- /dev/null +++ b/streets_utils/json_utils/test/test_files/valid.json @@ -0,0 +1,16 @@ +[ + { + "sensorId": "sensor_1", + "type": "SemanticLidar", + "location": { + "x": 1.0, + "y": 2.0, + "z": -3.2 + }, + "orientation": { + "yaw": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + } +] \ No newline at end of file diff --git a/streets_utils/streets_messages/CMakeLists.txt b/streets_utils/streets_messages/CMakeLists.txt new file mode 100644 index 000000000..f53daf8e6 --- /dev/null +++ b/streets_utils/streets_messages/CMakeLists.txt @@ -0,0 +1,85 @@ +# Copyright 2019-2023 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +cmake_minimum_required(VERSION 3.10.2) +project(streets_messages) +# Set Flags +set(CMAKE_CXX_STANDARD 17) +# GNU standard installation directories +include(GNUInstallDirs) +# Find Packages +find_package(spdlog REQUIRED) +find_package(RapidJSON REQUIRED) +find_package(GTest REQUIRED) +find_package(json_utils_lib REQUIRED) +# Add definition for rapidjson to include std::string +add_definitions(-DRAPIDJSON_HAS_STDSTRING=1) +set(LIBRARY_NAME ${PROJECT_NAME}_lib) +######################################################## +# Build Library +######################################################## +file(GLOB STREETS_MESSAGES_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/**/*.cpp) +add_library(${LIBRARY_NAME} ${STREETS_MESSAGES_SOURCES} ) +target_include_directories(${LIBRARY_NAME} + PUBLIC + $ +) +target_link_libraries( ${LIBRARY_NAME} + PUBLIC + spdlog::spdlog + rapidjson + streets_utils::json_utils_lib +) +######################################################## +# Install streets_utils::streets_messages package. +######################################################## +install( + TARGETS ${LIBRARY_NAME} + EXPORT ${LIBRARY_NAME}Targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/streets_utils/ + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/streets_utils/${LIBRARY_NAME} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/streets_utils/ +) +install( + EXPORT ${LIBRARY_NAME}Targets + FILE ${LIBRARY_NAME}Targets.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${LIBRARY_NAME} + NAMESPACE streets_utils:: +) +include(CMakePackageConfigHelpers) +configure_package_config_file( + cmake/${LIBRARY_NAME}Config.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${LIBRARY_NAME}) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${LIBRARY_NAME} +) +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ + DESTINATION include/streets_utils/${LIBRARY_NAME} + FILES_MATCHING PATTERN "*.hpp" # install header files including package path + ) + +######################## +# Setup Test executable +######################## +set(TEST_NAME ${LIBRARY_NAME}_test) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/**/*.hpp test/**/*.cpp) +set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +add_executable(${TEST_NAME} ${TEST_SOURCES}) +add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME}) +target_link_libraries(${TEST_NAME} + PRIVATE + ${LIBRARY_NAME} + GTest::Main + ) diff --git a/streets_utils/streets_messages/DetectedObjectsMessage.md b/streets_utils/streets_messages/DetectedObjectsMessage.md new file mode 100644 index 000000000..a4ee05e6e --- /dev/null +++ b/streets_utils/streets_messages/DetectedObjectsMessage.md @@ -0,0 +1,88 @@ +# Detected Objects Message + +## Introduction + +This CARMA-Streets library contains the Detected Objects Message as well as the logic for deserializing this object from JSON. Below is an example JSON payload. This message will be produced by an external Sensor capable of detecting objects. The data in it will represent the incoming **Sensor Detected Object** detection. This is a custom message definition for the detected objects. + +## Example JSON payload +``` +{ + "type": "CAR", + "confidence": 0.7, + "sensorId": "sensor1", + "projString": "projection String2", + "objectId": 123, + "position": { + "x": -1.1, + "y": -2, + "z": -3.2 + }, + "positionCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "velocity": { + "x": 1.0, + "y": 1.0, + "z": 1.0 + }, + "velocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "angularVelocity": { + "x": 0.1, + "y": 0.2, + "z": 0.3 + }, + "angularVelocityCovariance": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ] + ], + "size": { + "length": 2.0, + "height": 1.0, + "width": 0.5 + }, + "timestamp":1200 +} +``` diff --git a/streets_utils/streets_messages/README.md b/streets_utils/streets_messages/README.md new file mode 100644 index 000000000..c15ea8af0 --- /dev/null +++ b/streets_utils/streets_messages/README.md @@ -0,0 +1,45 @@ +# Streets Messages Library + +## Introduction + +This CARMA-Streets library will be the new location for storing both CARMA Streets message data types as well as the logic to serialize and deserialize these messages. Message data types will be stored in directories with the message name as the directory name similar to the current `sensor_data_sharing_msg`. Then functions will hold the JSON serialization and deserialization logic for each message to seperate concerns between message serialization and message data. +> [!IMPORTANT]\ +> Currently this library only contains the sensor_data sharing message and the detected objects message, with their JSON serialization logic. Will attempt to part existing and future messages into this library. + +## Messages +**[Sensor Data Sharing Message](SensorDataSharingMessage.md)** : J3224 Message used for sharing detection level data between vehicles and infrastructure. +**[Detected Objects Message](DetectedObjectsMessage.md)** : Custom Message used for sharing object detection information between sensors and ITS actors +## Serialization and Deserialization + +CARMA Streets messages are currently all using JSON for serialization for sending via a Apache Kafka broker. To parse or write the JSON we are currently using the [rapidjson library](https://github.com/Tencent/rapidjson). In addition, we have implemented some helper functions to parse optional fields from JSON using the [std::optional](https://en.cppreference.com/w/cpp/utility/optional) class template implemented in our [json_utils library](https://github.com/usdot-fhwa-stol/carma-streets/tree/develop/streets_utils/json_utils). + +## Some future planned improvements to this library include: +- Porting existing CARMA Streets message JSON serialization/deserialization into library functions and out of message data type definitions +- introduce "compile time inheritance" for serialization and deserialization to improve ease of use (https://github.com/usdot-fhwa-stol/carma-streets/issues/356) + +> [!IMPORTANT]\ +> This is an improvement but also a deviation from how messages were previously processed in which message data types were couple to serialization. To remove this coupling new library will separate message data types from functions that serialize/deserialize them. This provides multiple benefits. By separating the logic the serialization logic from the message data types this improves maintainability by allowing us to potentially change serialization without any direct impact on message types or logic using message data. It also reduces the number of packages dependent on external libraries related to serialization like rapidjson. + +## Installation +This library includes a CMake install target that will attempt to install this library, including CMake configuration files under the following general path: + +`streets_utils\streets_messages_lib` + +Header files will be installed in the CMake default include directory + +`include\streets_utils\streets_messages_lib\` + +CMake configuration files will be installed under CMake default install directory + +`cmake\streets_messages_lib\` + +And the library binary will be installed under the CMake default Library directory + +`lib\streets_utils\` + +## Using Library + +To incorporate this library into a CARMA Streets Service simply install the library using `make install` and then use the CMake `find_package(streets_messages REQUIRED)` call to find the install package. To link this library include the streets utils namespace as follows: +``` +target_link_libraries( PUBLIC streets_utils::streets_messages) +``` \ No newline at end of file diff --git a/streets_utils/streets_messages/SensorDataSharingMessage.md b/streets_utils/streets_messages/SensorDataSharingMessage.md new file mode 100644 index 000000000..4d0665086 --- /dev/null +++ b/streets_utils/streets_messages/SensorDataSharingMessage.md @@ -0,0 +1,95 @@ +# Sensor Data Sharing Message + +## Introduction + +This CARMA-Streets library contains the Sensor Data Sharing Object as well as the logic for serailizing and deserializing this object to and from JSON. Below is an example JSON payload. This message will be produced by the **Sensor Data Sharing** CARMA Streets micro-service. The data in it will represent the incoming **Sensor Detected Object** detection. This message will be generated inline with the SAE J3224 document. +> [!IMPORTANT]\ +> The first implementation of the SDSM generator will not include functionality to filter out detections that are likely self reporting actors publishing BSMs like described in the J3224 202208 document. +## Example JSON payload +```json +{ + "msg_cnt": 1, + "source_id": "00000001", + "equipment_type": 2, + "sdsm_time_stamp": { + "second": 0, + "minute": 0, + "hour": 0, + "day": 0, + "month": 0, + "year": 0, + "offset": -32767 + }, + "ref_pos": { + "long": -1800000000, + "lat": -90000000 + }, + "ref_pos_xy_conf": { + "semi_major": 0, + "semi_minor": 0, + "orientation": 0 + }, + "objects": [ + { + "detected_object_data": { + "detected_object_common_data": { + "obj_type": 1, + "object_id": 0, + "obj_type_cfd": 0, + "measurement_time": -1500, + "time_confidence": 39, + "pos": { + "offset_x": -32767, + "offset_y": -32767, + "offset_z": -32767 + }, + "pos_confidence": { + "pos": 1, + "elevation": 15 + }, + "speed": 0, + "speed_confidence": 21845, + "heading": 0, + "heading_conf": 7 + }, + "detected_object_optional_data": { + "detected_vehicle_data": { + "lights": "11110000", + "veh_attitude": { + "pitch": 72000, + "roll": 14400, + "yaw": 14400 + }, + "veh_attitude_confidence": { + "pitch_confidence": 5, + "roll_confidence": 1, + "yaw_confidence": 3 + }, + "veh_ang_vel": { + "pitch_rate": 32767, + "roll_rate": 32767 + }, + "veh_ang_vel_confidence": { + "pitch_rate_confidence": 4 + }, + "size": { + "width": 4095, + "length": 4095 + }, + "vehicle_size_confidence": { + "vehicle_width_confidence": 13, + "vehicle_length_confidence": 13, + "vehicle_height_confidence": 13 + }, + "height": 127, + "vehicle_class": 23, + "class_conf": 101 + } + } + } + } + ] +} +``` + + diff --git a/streets_utils/streets_messages/cmake/streets_messages_libConfig.cmake.in b/streets_utils/streets_messages/cmake/streets_messages_libConfig.cmake.in new file mode 100644 index 000000000..0b441c5ee --- /dev/null +++ b/streets_utils/streets_messages/cmake/streets_messages_libConfig.cmake.in @@ -0,0 +1,21 @@ +# Copyright 2019-2023 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(spdlog REQUIRED) +find_dependency(RapidJSON REQUIRED) +find_dependency(json_utils_lib REQUIRED) + +include("${CMAKE_CURRENT_LIST_DIR}/streets_messages_libTargets.cmake") diff --git a/streets_utils/streets_messages/include/deserializers/detected_obj_msg_deserializer.hpp b/streets_utils/streets_messages/include/deserializers/detected_obj_msg_deserializer.hpp new file mode 100644 index 000000000..27b96822e --- /dev/null +++ b/streets_utils/streets_messages/include/deserializers/detected_obj_msg_deserializer.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace streets_utils::messages::detected_objects_msg { + + + detected_objects_msg from_json( const std::string &val); + + cartesian_point parse_cartesian_3d(const rapidjson::Value &val); + + std::vector> parse_covariance(const rapidjson::Value::ConstArray &val); + + vector_3d parse_vector3d(const rapidjson::Value &val); + + size parse_size(const rapidjson::Value &val); + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/deserializers/sensor_data_sharing_msg_json_deserializer.hpp b/streets_utils/streets_messages/include/deserializers/sensor_data_sharing_msg_json_deserializer.hpp new file mode 100644 index 000000000..dd661ac83 --- /dev/null +++ b/streets_utils/streets_messages/include/deserializers/sensor_data_sharing_msg_json_deserializer.hpp @@ -0,0 +1,70 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include "sensor_data_sharing_msg/sensor_data_sharing_msg.hpp" + +namespace streets_utils::messages::sdsm { + sensor_data_sharing_msg from_json( const std::string &val); + + time_stamp parse_time_stamp(const rapidjson::Value &val); + + position_3d parse_position_3d(const rapidjson::Value &val); + + positional_accuracy parse_positional_accuracy(const rapidjson::Value &val); + + std::optional parse_elevation_confidence(const rapidjson::Value &val); + + std::vector parse_detected_object_list(const rapidjson::Value &val); + + detected_object_data_common parse_detected_object_data_common(const rapidjson::Value &val); + + acceleration_set_4_way parse_acceleration_4_way(const rapidjson::Value &val); + + std::optional> parse_detected_object_data_optional(const rapidjson::Value &val); + + position_offset parse_position_offset(const rapidjson::Value &val); + + position_confidence_set parse_position_confidence_set(const rapidjson::Value &val); + + detected_obstacle_data parse_detected_obstacle_data(const rapidjson::Value &val); + + detected_vehicle_data parse_detected_vehicle_data(const rapidjson::Value &val); + + detected_vru_data parse_detected_vru_data(const rapidjson::Value &val); + + obstacle_size parse_obstacle_size(const rapidjson::Value &val); + + obstacle_size_confidence parse_obstacle_size_confidence(const rapidjson::Value &val); + + std::optional> parse_propelled_information( const rapidjson::Value &val); + + attitude parse_vehicle_attitude(const rapidjson::Value &val); + + attitude_confidence parse_vehicle_attitude_confidence(const rapidjson::Value &val); + + angular_velocity_set parse_vehicle_angular_velocity_set(const rapidjson::Value &val); + + angular_velocity_confidence_set parse_vehicle_angular_velocity_confidence_set(const rapidjson::Value &val); + + vehicle_size parse_vehicle_size(const rapidjson::Value &val); + + vehicle_size_confidence parse_vehicle_size_confidence(const rapidjson::Value &val); +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/detected_object_msg/cartesian_point.hpp b/streets_utils/streets_messages/include/detected_object_msg/cartesian_point.hpp new file mode 100644 index 000000000..5b7b188fd --- /dev/null +++ b/streets_utils/streets_messages/include/detected_object_msg/cartesian_point.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include +#include + + +namespace streets_utils::messages::detected_objects_msg { + + + /** + * @brief Struct for world frame coordinates. + */ + struct cartesian_point{ + double _x; + double _y; + double _z; + }; + + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/detected_object_msg/detected_object_msg.hpp b/streets_utils/streets_messages/include/detected_object_msg/detected_object_msg.hpp new file mode 100644 index 000000000..f1918a479 --- /dev/null +++ b/streets_utils/streets_messages/include/detected_object_msg/detected_object_msg.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace streets_utils::messages::detected_objects_msg { + + /** + * @brief Sensor Detected Object information + */ + struct detected_objects_msg { + + std::string _type; + double _confidence; + std::string _sensor_id; + std::string _proj_string; + unsigned int _object_id; + cartesian_point _position; + std::vector> _position_covariance; + vector_3d _velocity; + std::vector> _velocity_covariance; + vector_3d _angular_velocity; + std::vector> _angular_velocity_covariance; + size _size; + uint64_t _timestamp; + + }; + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/detected_object_msg/size.hpp b/streets_utils/streets_messages/include/detected_object_msg/size.hpp new file mode 100644 index 000000000..b86b9dd46 --- /dev/null +++ b/streets_utils/streets_messages/include/detected_object_msg/size.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include +#include + + +namespace streets_utils::messages::detected_objects_msg { + + /** + * @brief Struct for size dimensions. + */ + struct size{ + + double _length; /**length in meters */ + double _height; /**height in meters */ + double _width; /**width in meters */ + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/detected_object_msg/vector_3d.hpp b/streets_utils/streets_messages/include/detected_object_msg/vector_3d.hpp new file mode 100644 index 000000000..40f062359 --- /dev/null +++ b/streets_utils/streets_messages/include/detected_object_msg/vector_3d.hpp @@ -0,0 +1,32 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include +#include + + +namespace streets_utils::messages::detected_objects_msg { + + /** + * @brief 3 dimensional vector struct. + */ + struct vector_3d{ + double _x; + double _y; + double _z; + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/acceleration_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/acceleration_confidence.hpp new file mode 100644 index 000000000..d9ba549ad --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/acceleration_confidence.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + enum class acceleration_confidence { + UNAVAILABLE = 0, // Not available + ACCL_100 = 1, // 100 m/s^2 + ACCL_10 = 2, // 10 m/s^2 + ACCL_5 = 3, // 5 m/s^2 + ACCL_1 = 4, // 1 m/s^2 + ACCL_0_1 = 5, // 0.1 m/s^2 + ACCL_0_05 = 6, // 0.05 m/s^2 + ACCL_0_01 = 7 // 0.01 m/s^2 + }; + + inline acceleration_confidence acceleration_confidence_from_int( const unsigned int i ) { + switch (i) + { + case 0: + return acceleration_confidence::UNAVAILABLE; + case 1: + return acceleration_confidence::ACCL_100; + case 2: + return acceleration_confidence::ACCL_10; + case 3: + return acceleration_confidence::ACCL_5; + case 4: + return acceleration_confidence::ACCL_1; + case 5: + return acceleration_confidence::ACCL_0_1; + case 6: + return acceleration_confidence::ACCL_0_05; + case 7: + return acceleration_confidence::ACCL_0_01; + default: + throw std::invalid_argument("Incompatible acceleration confidence value. Valid values : [0,7]"); + } + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/acceleration_set_4_way.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/acceleration_set_4_way.hpp new file mode 100644 index 000000000..cb4e599e6 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/acceleration_set_4_way.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + + +namespace streets_utils::messages::sdsm{ + struct acceleration_set_4_way{ + /** + * @brief Longitudinal acceleration in 0.01 m/s^s [-2000, 2001] + */ + int _longitudinal_accel; + /** + * @brief Lateral acceleration in 0.01 m/s^s [-2000, 2001] + */ + int _lateral_accel; + /** + * @brief Vertical acceleration in 0.02 G [-127, 127] + */ + int _vertical_accel; + /** + * @brief Angular velocity in 0.01 degrees [-32767, 32767] + */ + int _yaw_rate; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/angular_velocity_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/angular_velocity_confidence.hpp new file mode 100644 index 000000000..9916f5165 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/angular_velocity_confidence.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + enum class angular_velocity_confidence{ + UNAVAILABLE = 0, + DEGSEC_100 = 1, // 100 degrees + DEGSEC_10 = 2, // 10 degrees + DEGSEC_05 = 3, // 5 degrees + DEGSEC_01 = 4, // 1 degrees + DEGSEC_0_1 = 5, // 0.1 degrees + DEGSEC_0_05 = 6, // 0.05 degrees + DEGSEC_0_01 = 7, // 0.01 degrees + }; + + inline angular_velocity_confidence angular_velocity_confidence_from_int( const unsigned int i ){ + switch (i) + { + case 0: + return angular_velocity_confidence::UNAVAILABLE; + case 1: + return angular_velocity_confidence::DEGSEC_100; + case 2: + return angular_velocity_confidence::DEGSEC_10; + case 3: + return angular_velocity_confidence::DEGSEC_05; + case 4: + return angular_velocity_confidence::DEGSEC_01; + case 5: + return angular_velocity_confidence::DEGSEC_0_1; + case 6: + return angular_velocity_confidence::DEGSEC_0_05; + case 7: + return angular_velocity_confidence::DEGSEC_0_01; + default: + throw std::invalid_argument("Incompatible angular velocity confidence value. Valid values : [0,7]"); + } + }; + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/detected_object_data.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/detected_object_data.hpp new file mode 100644 index 000000000..87f4b552e --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/detected_object_data.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/detected_object_data_common.hpp" +#include "sensor_data_sharing_msg/obstacle/detected_obstacle_data.hpp" +#include "sensor_data_sharing_msg/vehicle/detected_vehicle_data.hpp" +#include "sensor_data_sharing_msg/vru/detected_vru_data.hpp" +#include + +namespace streets_utils::messages::sdsm{ + struct detected_object_data { + /** + * @brief Common data for detected object. + */ + detected_object_data_common _detected_object_common_data; + /** + * @brief Detected object optional data associated with object type classification. + */ + std::optional> _detected_object_optional_data; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/detected_object_data_common.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/detected_object_data_common.hpp new file mode 100644 index 000000000..a4f70e021 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/detected_object_data_common.hpp @@ -0,0 +1,96 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/object_type.hpp" +#include "sensor_data_sharing_msg/time_confidence.hpp" +#include "sensor_data_sharing_msg/position_offset.hpp" +#include "sensor_data_sharing_msg/position_confidence_set.hpp" +#include "sensor_data_sharing_msg/speed_confidence.hpp" +#include "sensor_data_sharing_msg/heading_confidence.hpp" +#include "sensor_data_sharing_msg/acceleration_set_4_way.hpp" +#include "sensor_data_sharing_msg/acceleration_confidence.hpp" +#include "sensor_data_sharing_msg/angular_velocity_confidence.hpp" +#include +#include + +namespace streets_utils::messages::sdsm { + struct detected_object_data_common{ + /** + * @brief Object type enumeration + */ + object_type _object_type = object_type::UNKNOWN; + /** + * @brief Confidence in object type classification [0,101] + */ + unsigned int _classification_confidence = 0; + /** + * @brief Object ID [0, 65535] + */ + unsigned int _object_id = 0; + /** + * @brief Time relative to SDSM timestamp assoicated with detection [-1500, 1500] + */ + int _time_measurement_offset = 0; + /** + * @brief Time Confidence enumeration for time offset. + */ + time_confidence _time_confidence = time_confidence::UNAVAILABLE; + /** + * @brief Cartesian offset from SDSM reporter reference location to represent detected object location + */ + position_offset _position_offset; + /** + * @brief Confidence in reported position + */ + position_confidence_set _pos_confidence; + /** + * @brief Object speed in unit (0.02 m/s) [0, 8191] + */ + unsigned int _speed = 0; + /** + * @brief Confidence in reported speed + */ + speed_confidence _speed_confidence = speed_confidence::UNAVAILABLE; + /** + * @brief Object speed along Z axis unit (0.02 m/s) [0, 8191] + */ + std::optional _speed_z; + /** + * @brief Confidence in reported speed z + */ + std::optional _speed_z_confidence; + /** + * @brief Heading in 0.0125 degrees [0, 28800] + */ + unsigned int _heading = 0; + /** + * @brief Confidence in reported heading + */ + heading_confidence _heading_confidence = heading_confidence::UNAVAILABLE; + /** + * @brief Acceleration in longitudinal, lateral, vertical and angular velocity. + */ + std::optional _acceleration_4_way; + + std::optional _longitudinal_acceleration_confidence; + + std::optional _lateral_acceleration_confidence; + + std::optional _vertical_accelaration_confidence; + + std::optional _yaw_rate_confidence; + + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/equipment_type.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/equipment_type.hpp new file mode 100644 index 000000000..a82911346 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/equipment_type.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + + +namespace streets_utils::messages::sdsm{ + + enum class equipment_type { + UNKNOWN = 0, + RSU = 1, + OBU= 2, + VRU= 3 + }; + /** + * @brief Function to convert integers to equiment type. Default to UNKNOWN. + * @param i integer value of enum. + * @return corresponding equipement type. + */ + inline equipment_type equipment_type_from_int( const unsigned int i ) { + switch (i) + { + case 0: + return equipment_type::UNKNOWN; + case 1: + return equipment_type::RSU; + case 2: + return equipment_type::OBU; + case 3: + return equipment_type::VRU; + default: + throw std::invalid_argument("Incompatible equipment type value. Valid values : [0,3]"); + } + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/heading_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/heading_confidence.hpp new file mode 100644 index 000000000..b83e5fbf8 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/heading_confidence.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + enum class heading_confidence{ + UNAVAILABLE = 0, + PREC_10_deg = 1, // 10 degrees + PREC_05_deg = 2, // 5 degrees + PREC_01_deg = 3, // 1 degrees + PREC_0_1_deg = 4, // 0.1 degrees + PREC_0_05_deg = 5, // 0.05 degrees + PREC_0_01_deg = 6, // 0.01 degrees + PREC_0_0125_deg = 7 // 0.0125 degrees + }; + + inline heading_confidence heading_confidence_from_int( const unsigned int i ) { + switch (i) + { + case 0: + return heading_confidence::UNAVAILABLE; + case 1: + return heading_confidence::PREC_10_deg; + case 2: + return heading_confidence::PREC_05_deg; + case 3: + return heading_confidence::PREC_01_deg; + case 4: + return heading_confidence::PREC_0_1_deg; + case 5: + return heading_confidence::PREC_0_05_deg; + case 6: + return heading_confidence::PREC_0_01_deg; + case 7: + return heading_confidence::PREC_0_0125_deg; + default: + throw std::invalid_argument("Incompatible heading confidence value. Valid values : [0,7]"); + } + }; + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/object_type.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/object_type.hpp new file mode 100644 index 000000000..cf7da54d1 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/object_type.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + + enum class object_type { + UNKNOWN = 0, + VEHICLE = 1, + VRU= 2, + ANIMAL= 3 + }; + + inline object_type object_type_from_int( const int i ) { + switch (i) + { + case 0: + return object_type::UNKNOWN; + case 1: + return object_type::VEHICLE; + case 2: + return object_type::VRU; + case 3: + return object_type::ANIMAL; + default: + throw std::invalid_argument("Incompatible object type value. Valid values : [0,3]"); + } + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/detected_obstacle_data.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/detected_obstacle_data.hpp new file mode 100644 index 000000000..1c4fda26e --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/detected_obstacle_data.hpp @@ -0,0 +1,29 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/obstacle/obstacle_size.hpp" +#include "sensor_data_sharing_msg/obstacle/obstacle_size_confidence.hpp" +namespace streets_utils::messages::sdsm{ + struct detected_obstacle_data{ + /** + * @brief Size of obstacle. + */ + obstacle_size _size; + /** + * @brief Confidence of reported size. + */ + obstacle_size_confidence _size_confidence; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/obstacle_size.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/obstacle_size.hpp new file mode 100644 index 000000000..bd2bd5911 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/obstacle_size.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include + +namespace streets_utils::messages::sdsm { + struct obstacle_size { + /** + * @brief Object width in 10 cm units [0, 1023]. + */ + unsigned int _width = 0; + /** + * @brief Object length in 10 cm units [0, 1023] + */ + unsigned int _length = 0; + /** + * @brief **Optional** Object height in 10 cm units [0, 1023] + */ + std::optional _height; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/obstacle_size_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/obstacle_size_confidence.hpp new file mode 100644 index 000000000..bd8e60a9a --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/obstacle/obstacle_size_confidence.hpp @@ -0,0 +1,32 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once +#include "sensor_data_sharing_msg/size_value_confidence.hpp" +#include +namespace streets_utils::messages::sdsm{ + struct obstacle_size_confidence{ + /** + * @brief Confidence in reported width + */ + size_value_confidence _width_confidence = size_value_confidence::UNAVAILABLE; + /** + * @brief Confidence in reported length + */ + size_value_confidence _length_confidence = size_value_confidence::UNAVAILABLE; + /** + * @brief Confidence in reported height + */ + std::optional _height_confidence; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_3d.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_3d.hpp new file mode 100644 index 000000000..34785aee1 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_3d.hpp @@ -0,0 +1,38 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include + +namespace streets_utils::messages::sdsm{ + struct position_3d{ + /** + * @brief LSB = 1/10 micro degree Providing a range of + * plus-minus 180 degrees[-1799999999, 1800000001] + */ + int _longitude = 0; + /** + * @brief LSB = 1/10 micro degree Providing a range of + * plus-minus 90 degrees[-900000000, 900000001] + */ + int _latitude = 0; + /** + * @brief Signed units of 0.1m (10cm), in 2 octets the value + * 32767 (0x7FFF) shall indicate an invalid value [-32768,32767] + */ + std::optional _elevation; + + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_confidence.hpp new file mode 100644 index 000000000..62f3dccd5 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_confidence.hpp @@ -0,0 +1,78 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm { + enum class position_confidence{ + UNAVAILABLE = 0, + A_500M = 1, + A_200M = 2, + A_100M = 3, + A_50M = 4, + A_20M = 5, + A_10M = 6, + A_5M = 7, + A_2M = 8, + A_1M = 9, + A_50CM = 10, + A_20CM = 11, + A_10CM = 12, + A_5CM = 13, + A_2CM = 14, + A_1CM = 15 + + }; + + inline position_confidence position_confidence_from_int( const int i ) { + switch (i) + { + case 0: + return position_confidence::UNAVAILABLE; + case 1: + return position_confidence::A_500M; + case 2: + return position_confidence::A_200M; + case 3: + return position_confidence::A_100M; + case 4: + return position_confidence::A_50M; + case 5: + return position_confidence::A_20M; + case 6: + return position_confidence::A_10M; + case 7: + return position_confidence::A_5M; + case 8: + return position_confidence::A_2M; + case 9: + return position_confidence::A_1M; + case 10: + return position_confidence::A_50CM; + case 11: + return position_confidence::A_20CM; + case 12: + return position_confidence::A_10CM; + case 13: + return position_confidence::A_5CM; + case 14: + return position_confidence::A_2CM; + case 15: + return position_confidence::A_1CM; + default: + throw std::invalid_argument("Incompatible position confidence value. Valid values : [0,15]"); + } + }; + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_confidence_set.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_confidence_set.hpp new file mode 100644 index 000000000..a66dbaa4c --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_confidence_set.hpp @@ -0,0 +1,24 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/position_confidence.hpp" + +namespace streets_utils::messages::sdsm { + struct position_confidence_set{ + position_confidence _position_confidence = position_confidence::UNAVAILABLE; + position_confidence _elevation_confidence = position_confidence::UNAVAILABLE; + + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_offset.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_offset.hpp new file mode 100644 index 000000000..4bbd0ecb3 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/position_offset.hpp @@ -0,0 +1,34 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include + +namespace streets_utils::messages::sdsm{ + struct position_offset{ + /** + * @brief Cartesian offset in X axis from reference point in 0.1 m [-32767, 32767] + */ + int _offset_x = 0; + /** + * @brief Cartesian offset in Y axis from reference point in 0.1 m [-32767, 32767] + */ + int _offset_y = 0; + /** + * @brief Cartesian offset in Z axis from reference point in 0.1 m [-32767, 32767] + */ + std::optional _offset_z; + + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/positional_accuracy.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/positional_accuracy.hpp new file mode 100644 index 000000000..de0018085 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/positional_accuracy.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + + +namespace streets_utils::messages::sdsm { + struct positional_accuracy{ + /** + * @brief semi-major axis accuracy at one standard dev + * range 0-12.7 meter, LSB = .05m [0, 255] + * 254 = any value equal or greater than 12.70 meter + * 255 = unavailable semi-major axis value + */ + unsigned int _semi_major_axis_accuracy = 0; + /** + * @brief semi-minor axis accuracy at one standard dev + * range 0-12.7 meter, LSB = .05m [0, 255] + * 254 = any value equal or greater than 12.70 meter + * 255 = unavailable semi-minor axis value + */ + unsigned int _semi_minor_axis_accuracy = 0; + /** + * @brief -- orientation of semi-major axis + * relative to true north (0~359.9945078786 degrees) + * LSB units of 360/65535 deg = 0.0054932479 [0,65535] + * a value of 0 shall be 0 degrees + * a value of 1 shall be 0.0054932479 degrees + * a value of 65534 shall be 359.9945078786 deg + * a value of 65535 shall be used for orientation unavailable + */ + unsigned int _semi_major_axis_orientation = 0; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/sensor_data_sharing_msg.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/sensor_data_sharing_msg.hpp new file mode 100644 index 000000000..c76baa709 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/sensor_data_sharing_msg.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/equipment_type.hpp" +#include "sensor_data_sharing_msg/time_stamp.hpp" +#include "sensor_data_sharing_msg/detected_object_data.hpp" +#include "sensor_data_sharing_msg/position_3d.hpp" +#include "sensor_data_sharing_msg/positional_accuracy.hpp" + +#include +#include +#include + +namespace streets_utils::messages::sdsm { + + + /** + * @brief + */ + struct sensor_data_sharing_msg { + /** + * @brief -- a count value which is incremented with each use [0,255] + * the next value after 255 shall be one value + * 0 (0x00) shall indicate that MsgCount is not available. + */ + std::size_t _msg_count = 0; + equipment_type _equipment_type = equipment_type::UNKNOWN; + position_3d _ref_positon; + std::optional _ref_position_elevation_confidence; + positional_accuracy _ref_position_confidence; + time_stamp _time_stamp; + std::string _source_id; + std::vector _objects; + + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/size_value_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/size_value_confidence.hpp new file mode 100644 index 000000000..33e98e4f2 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/size_value_confidence.hpp @@ -0,0 +1,69 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + enum class size_value_confidence{ + UNAVAILABLE = 0, // Not available + SIZE_100 = 1, // 100 meters + SIZE_50 = 2, // 50 meters + SIZE_20 = 3, // 20 meters + SIZE_10 = 4, // 10 meters + SIZE_5 = 5, // 5 meters + SIZE_2 = 6, // 2 meters + SIZE_1 = 7, // 1 meter + SIZE_0_5 = 8, // 50 centimeters + SIZE_0_2 = 9, // 20 centimeters + SIZE_0_1 = 10, // 10 centimeters + SIZE_0_05 = 11, // 5 centimeters + SIZE_0_02 = 12, // 2 centimeters + SIZE_0_01 = 13 // 1 centimeters + }; + + inline size_value_confidence size_value_confidence_from_int( const unsigned int i ) { + switch (i) + { + case 0: + return size_value_confidence::UNAVAILABLE; + case 1: + return size_value_confidence::SIZE_100; + case 2: + return size_value_confidence::SIZE_50; + case 3: + return size_value_confidence::SIZE_20; + case 4: + return size_value_confidence::SIZE_10; + case 5: + return size_value_confidence::SIZE_5; + case 6: + return size_value_confidence::SIZE_2; + case 7: + return size_value_confidence::SIZE_1; + case 8: + return size_value_confidence::SIZE_0_5; + case 9: + return size_value_confidence::SIZE_0_2; + case 10: + return size_value_confidence::SIZE_0_1; + case 11: + return size_value_confidence::SIZE_0_05; + case 12: + return size_value_confidence::SIZE_0_02; + case 13: + return size_value_confidence::SIZE_0_01; + default: + throw std::invalid_argument("Incompatible size confidence value. Valid values : [0,13]"); + } + } +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/speed_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/speed_confidence.hpp new file mode 100644 index 000000000..76c238691 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/speed_confidence.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + enum class speed_confidence { + UNAVAILABLE = 0, // Not available + PREC_100ms = 1, // 100 m/s + PREC_10ms = 2, // 10 m/s + PREC_5ms = 3, // 5 m/s + PREC_1ms = 4, // 1 m/s + PREC_0_1ms = 5, // 0.1 m/s + PREC_0_05ms = 6, // 0.05 m/s + PREC_0_01ms = 7 // 0.01 m/s + }; + + inline speed_confidence speed_confidence_from_int( const unsigned int i ) { + switch (i) + { + case 0: + return speed_confidence::UNAVAILABLE; + case 1: + return speed_confidence::PREC_100ms; + case 2: + return speed_confidence::PREC_10ms; + case 3: + return speed_confidence::PREC_5ms; + case 4: + return speed_confidence::PREC_1ms; + case 5: + return speed_confidence::PREC_0_1ms; + case 6: + return speed_confidence::PREC_0_05ms; + case 7: + return speed_confidence::PREC_0_01ms; + default: + throw std::invalid_argument("Incompatible speed confidence value. Valid values : [0,7]"); + } + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/time_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/time_confidence.hpp new file mode 100644 index 000000000..8e30f030c --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/time_confidence.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + enum class time_confidence{ + UNAVAILABLE = 0, // unvailable + TIME_100_000 = 1, // Better than 100 seconds + TIME_050_000 = 2, // Better than 50 seconds + TIME_020_000 = 3, // Better than 20 seconds + TIME_010_000 = 4, // Better than 10 seconds + TIME_002_000 = 5, // Better than 2 seconds + TIME_001_000 = 6, // Better than 1 seconds + TIME_000_500 = 7, // Better than 0.5 seconds + TIME_000_200 = 8, // Better than 0.2 seconds + TIME_000_100 = 9, // Better than 0.1 seconds + TIME_000_050 = 10, // Better than 0.05 seconds + TIME_000_020 = 11, // Better than 0.02 seconds + TIME_000_010 = 12, // Better than 0.01 seconds + TIME_000_005 = 13, // Better than 0.005 seconds + TIME_000_002 = 14, // Better than 0.002 seconds + TIME_000_001 = 15, // Better than 0.001 seconds + TIME_000_000_5 = 16, // Better than 0.0005 seconds + TIME_000_000_2 = 17, // Better than 0.0002 seconds + TIME_000_000_1 = 18, // Better than 0.0001 seconds + TIME_000_000_05 = 19, // Better than 0.00005 seconds + TIME_000_000_02 = 20, // Better than 0.00002 seconds + TIME_000_000_01 = 21, // Better than 0.00001 seconds + TIME_000_000_005 = 22, // Better than 0.000005 seconds + TIME_000_000_002 = 23, // Better than 0.000002 seconds + TIME_000_000_001 = 24, // Better than 0.000001 seconds + TIME_000_000_000_5 = 25, // Better than 0.0000005 seconds + TIME_000_000_000_2 = 26, // Better than 0.0000002 seconds + TIME_000_000_000_1 = 27, // Better than 0.0000001 seconds + TIME_000_000_000_05 = 28, // Better than 0.00000005 seconds + TIME_000_000_000_02 = 29, // Better than 0.00000002 seconds + TIME_000_000_000_01 = 30, // Better than 0.00000001 seconds + TIME_000_000_000_005 = 31, // Better than 0.000000005 seconds + TIME_000_000_000_002 = 32, // Better than 0.000000002 seconds + TIME_000_000_000_001 = 33, // Better than 0.000000001 seconds (Better than one nano second) + TIME_000_000_000_000_5 = 34,// Better than 0.0000000005 seconds + TIME_000_000_000_000_2 = 35,// Better than 0.0000000002 seconds + TIME_000_000_000_000_1 = 36,// Better than 0.0000000001 seconds + TIME_000_000_000_000_05 = 37, // Better than 0.00000000005 seconds + TIME_000_000_000_000_02 = 38, // Better than 0.00000000002 seconds + TIME_000_000_000_000_01 = 39 // Better than 0.00000000001 Seconds + }; + + inline time_confidence time_confidence_from_int( const int i ) { + switch (i) + { + case 0: + return time_confidence::UNAVAILABLE; + case 1: + return time_confidence::TIME_100_000; + case 2: + return time_confidence::TIME_050_000 ; + case 3: + return time_confidence::TIME_020_000 ; + case 4: + return time_confidence::TIME_010_000 ; + case 5: + return time_confidence::TIME_002_000 ; + case 6: + return time_confidence::TIME_001_000 ; + case 7: + return time_confidence::TIME_000_500 ; + case 8: + return time_confidence::TIME_000_200 ; + case 9: + return time_confidence::TIME_000_100 ; + case 10: + return time_confidence::TIME_000_050 ; + case 11: + return time_confidence::TIME_000_020 ; + case 12: + return time_confidence::TIME_000_010 ; + case 13: + return time_confidence::TIME_000_005 ; + case 14: + return time_confidence::TIME_000_002 ; + case 15: + return time_confidence::TIME_000_001 ; + case 16: + return time_confidence::TIME_000_000_5; + case 17: + return time_confidence::TIME_000_000_2; + case 18: + return time_confidence::TIME_000_000_1; + case 19: + return time_confidence::TIME_000_000_05; + case 20: + return time_confidence::TIME_000_000_02; + case 21: + return time_confidence::TIME_000_000_01; + case 22: + return time_confidence::TIME_000_000_005; + case 23: + return time_confidence::TIME_000_000_002; + case 24: + return time_confidence::TIME_000_000_001; + case 25: + return time_confidence::TIME_000_000_000_5; + case 26: + return time_confidence::TIME_000_000_000_2; + case 27: + return time_confidence::TIME_000_000_000_1; + case 28: + return time_confidence::TIME_000_000_000_05; + case 29: + return time_confidence::TIME_000_000_000_02; + case 30: + return time_confidence::TIME_000_000_000_01; + case 31: + return time_confidence::TIME_000_000_000_005; + case 32: + return time_confidence::TIME_000_000_000_002; + case 33: + return time_confidence::TIME_000_000_000_001; + case 34: + return time_confidence::TIME_000_000_000_000_5; + case 35: + return time_confidence::TIME_000_000_000_000_2; + case 36: + return time_confidence::TIME_000_000_000_000_1; + case 37: + return time_confidence::TIME_000_000_000_000_05; + case 38: + return time_confidence::TIME_000_000_000_000_02; + case 39: + return time_confidence::TIME_000_000_000_000_01; + default: + throw std::invalid_argument("Incompatible time confidence value. Valid values : [0,39]"); + } + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/time_stamp.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/time_stamp.hpp new file mode 100644 index 000000000..72f4c992f --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/time_stamp.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm { + struct time_stamp { + /** + * @brief in milliseconds [0,65535]. + */ + unsigned int second = 0; + /** + * @brief in minutes [0,60]. + */ + unsigned int minute = 0; + /** + * @brief in hours [0,31]. + */ + unsigned int hour = 0; + /** + * @brief in days [0,31]. + */ + unsigned int day = 0; + /** + * @brief in months [0,12]. + */ + unsigned int month = 0; + /** + * @brief in year [0,4095] + */ + unsigned int year = 0; + /** + * @brief Minutes from UTC time (Time Zone) [-840, 840] + */ + int offset = 0; // Time zone + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/angular_velocity_confidence_set.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/angular_velocity_confidence_set.hpp new file mode 100644 index 000000000..69a97e343 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/angular_velocity_confidence_set.hpp @@ -0,0 +1,30 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/angular_velocity_confidence.hpp" +#include + +namespace streets_utils::messages::sdsm{ + struct angular_velocity_confidence_set{ + /** + * @brief Confidence in reported pitch rate. + */ + std::optional _pitch_rate_confidence; + /** + * @brief Confidence in reported roll rate. + */ + std::optional _roll_rate_confidence; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/angular_velocity_set.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/angular_velocity_set.hpp new file mode 100644 index 000000000..467b6c118 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/angular_velocity_set.hpp @@ -0,0 +1,27 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +namespace streets_utils::messages::sdsm{ + struct angular_velocity_set{ + /** + * @brief Angular velocity for pitch axis in 0.01 degrees per second [-32767, 32767] + */ + int _pitch_rate = 0; + /** + * @brief Angular velocity for roll axis in 0.01 degrees per second [-32767, 32767] + */ + int _roll_rate = 0; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/attitude.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/attitude.hpp new file mode 100644 index 000000000..3b90ec685 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/attitude.hpp @@ -0,0 +1,32 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + + +namespace streets_utils::messages::sdsm{ + struct attitude{ + /** + * @brief Pitch in 0.0125 degrees [-7200, 72000]. + */ + int _pitch = 0; + /** + * @brief Roll in 0.0125 degrees [-14400, 14400] + */ + int _roll = 0; + /** + * @brief Yaw in 0.0125 degrees [-14400, 14400] + */ + int _yaw = 0; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/attitude_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/attitude_confidence.hpp new file mode 100644 index 000000000..7280b85d8 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/attitude_confidence.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/heading_confidence.hpp" + +namespace streets_utils::messages::sdsm{ + struct attitude_confidence { + /** + * @brief Confidence in reported pitch. + */ + heading_confidence _pitch_confidence = heading_confidence::UNAVAILABLE; + /** + * @brief Confidence in reported roll. + */ + heading_confidence _roll_confidence = heading_confidence::UNAVAILABLE; + /** + * @brief Confidence in reported yaw. + */ + heading_confidence _yaw_confidence = heading_confidence::UNAVAILABLE; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/detected_vehicle_data.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/detected_vehicle_data.hpp new file mode 100644 index 000000000..428fb769d --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/detected_vehicle_data.hpp @@ -0,0 +1,79 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include "sensor_data_sharing_msg/vehicle/attitude.hpp" +#include "sensor_data_sharing_msg/vehicle/attitude_confidence.hpp" +#include "sensor_data_sharing_msg/vehicle/angular_velocity_set.hpp" +#include "sensor_data_sharing_msg/vehicle/angular_velocity_confidence_set.hpp" +#include "sensor_data_sharing_msg/vehicle/vehicle_size.hpp" +#include "sensor_data_sharing_msg/vehicle/vehicle_size_confidence.hpp" + +namespace streets_utils::messages::sdsm{ + struct detected_vehicle_data{ + /** + * @brief BIT String representing the state of each of the following vehicle + * exterior lights: + * lowBeamHeadlightsOn (0), + * highBeamHeadlightsOn (1), + * leftTurnSignalOn (2), + * rightTurnSignalOn (3), + * hazardSignalOn (4), + * automaticLightControlOn (5), + * daytimeRunningLightsOn (6), + * fogLightOn (7), + * parkingLightsOn (8) + * + */ + std::optional exterior_lights; + /** + * @brief Vehicle Attitude. + */ + std::optional _veh_attitude; + /** + * @brief Confidence in reported vehicle attitude. + */ + std::optional _attitude_confidence; + /** + * @brief Angular velocity set in pitch and roll axis. + */ + std::optional _angular_velocity; + /** + * @brief Confidence in reported angular velocity set. + */ + std::optional _angular_velocity_confidence; + /** + * @brief Vehicle two dimensional size. + */ + std::optional _size; + /** + * @brief Vehicle height in unit of 5 cm [0, 127] + */ + std::optional _vehicle_height; + /** + * @brief Confidence in reported size. + */ + std::optional _size_confidence; + /** + * @brief See BasicVehicleClass in J2735 + */ + std::optional _vehicle_class; + /** + * @brief Confidence in vehicle classification [0,101] + */ + std::optional _classification_confidence; + + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/vehicle_size.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/vehicle_size.hpp new file mode 100644 index 000000000..285756122 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/vehicle_size.hpp @@ -0,0 +1,28 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + struct vehicle_size { + /** + * @brief Vehicle width in centimeters [0, 1023] + */ + unsigned int _width = 0; + /** + * @brief Vehicle length in centimeters [0, 4095] + */ + unsigned int _length = 0; + + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/vehicle_size_confidence.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/vehicle_size_confidence.hpp new file mode 100644 index 000000000..afa8e2007 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vehicle/vehicle_size_confidence.hpp @@ -0,0 +1,32 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once +#include "sensor_data_sharing_msg/size_value_confidence.hpp" +#include +namespace streets_utils::messages::sdsm{ + struct vehicle_size_confidence { + /** + * @brief Confidence in reported width + */ + size_value_confidence _width_confidence = size_value_confidence::UNAVAILABLE; + /** + * @brief Confidence in reported length + */ + size_value_confidence _length_confidence = size_value_confidence::UNAVAILABLE; + /** + * @brief Confidence in reported height + */ + std::optional _height_confidence; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/animal_propelled_type.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/animal_propelled_type.hpp new file mode 100644 index 000000000..2aca3151d --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/animal_propelled_type.hpp @@ -0,0 +1,41 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm{ + + enum class animal_propelled_type{ + UNAVAILABLE = 0, + OTHER_TYPES = 1, + ANIMAL_MOUNTED = 2, + ANIMAL_DRAWN_CARRIAGE = 3, + }; + + inline animal_propelled_type animal_propelled_type_from_int( const unsigned int i ) { + switch (i) + { + case 0: + return animal_propelled_type::UNAVAILABLE; + case 1: + return animal_propelled_type::OTHER_TYPES; + case 2: + return animal_propelled_type::ANIMAL_MOUNTED; + case 3: + return animal_propelled_type::ANIMAL_DRAWN_CARRIAGE; + default: + throw std::invalid_argument("Incompatible animal propelled type value. Valid values : [0,3]"); + } + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/attachment.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/attachment.hpp new file mode 100644 index 000000000..eb99127ac --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/attachment.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm { + enum class attachment { + UNAVAILABLE = 0 , + STROLLER = 1, + BICYLE_TRAILER = 2, + CART = 3, + WHEEL_CHAIR = 4, + OTHER_WALK_ASSIST_ATTACHMENTS = 5, + PET = 6 + }; + + inline attachment attachment_from_int( const int i ){ + switch (i) + { + case 0: + return attachment::UNAVAILABLE; + case 1: + return attachment::STROLLER; + case 2: + return attachment::BICYLE_TRAILER; + case 3: + return attachment::CART; + case 4: + return attachment::WHEEL_CHAIR; + case 5: + return attachment::OTHER_WALK_ASSIST_ATTACHMENTS; + case 6: + return attachment::PET; + default: + throw std::invalid_argument("Incompatible attachment value. Valid values : [0,6]"); + } + } + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/detected_vru_data.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/detected_vru_data.hpp new file mode 100644 index 000000000..85b492a91 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/detected_vru_data.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include "sensor_data_sharing_msg/vru/human_propelled_type.hpp" +#include "sensor_data_sharing_msg/vru/animal_propelled_type.hpp" +#include "sensor_data_sharing_msg/vru/motorized_propelled_type.hpp" +#include "sensor_data_sharing_msg/vru/attachment.hpp" +#include "sensor_data_sharing_msg/vru/personal_device_user_type.hpp" + +#include +#include + +namespace streets_utils::messages::sdsm{ + struct detected_vru_data{ + /** + * @brief Propulsion type. + */ + std::optional> _propulsion; + /** + * @brief Attachment type + */ + std::optional _attachment; + /** + * @brief Attachment radius in decimeters [0,200] + */ + std::optional _attachment_radius; + /** + * @brief Personal device user type + */ + std::optional _personal_device_user_type; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/human_propelled_type.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/human_propelled_type.hpp new file mode 100644 index 000000000..1cd86e342 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/human_propelled_type.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm { + + enum class human_propelled_type{ + UNAVAILABLE = 0, + OTHER_TYPES = 1, + ON_FOOT = 2, + SKATEBOARD = 3, + PUSH_OR_KICK_SCOOTER = 4, + WHEELCHAIR = 5 + }; + + inline human_propelled_type human_propelled_type_from_int( const int i ){ + switch (i) + { + case 0: + return human_propelled_type::UNAVAILABLE; + case 1: + return human_propelled_type::OTHER_TYPES; + case 2: + return human_propelled_type::ON_FOOT; + case 3: + return human_propelled_type::SKATEBOARD; + case 4: + return human_propelled_type::PUSH_OR_KICK_SCOOTER; + case 5: + return human_propelled_type::WHEELCHAIR; + default: + throw std::invalid_argument("Incompatible human propelled type value. Valid values : [0,5]"); + } + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/motorized_propelled_type.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/motorized_propelled_type.hpp new file mode 100644 index 000000000..a4c854760 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/motorized_propelled_type.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm { + enum class motorized_propelled_type{ + UNAVAILABLE = 0, + OTHER_TYPES = 1, + WHEEL_CHAIR = 2, + BICYCLE = 3, + SCOOTER = 4, + SELF_BALANCING_DEVICE = 5 + }; + + inline motorized_propelled_type motorized_propelled_type_from_int( const unsigned int i ){ + switch (i) + { + case 0: + return motorized_propelled_type::UNAVAILABLE; + case 1: + return motorized_propelled_type::OTHER_TYPES; + case 2: + return motorized_propelled_type::WHEEL_CHAIR; + case 3: + return motorized_propelled_type::BICYCLE; + case 4: + return motorized_propelled_type::SCOOTER; + case 5: + return motorized_propelled_type::SELF_BALANCING_DEVICE; + default: + throw std::invalid_argument("Incompatible motorized propelled type value. Valid values : [0,5]"); + } + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/personal_device_user_type.hpp b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/personal_device_user_type.hpp new file mode 100644 index 000000000..c2ef34579 --- /dev/null +++ b/streets_utils/streets_messages/include/sensor_data_sharing_msg/vru/personal_device_user_type.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +namespace streets_utils::messages::sdsm { + + enum class personal_device_user_type{ + UNAVAILABLE= 0, + PEDESTRIAN = 1, + PEDALCYCLIST = 2, + PUBLIC_SAFETY_WORKER = 3, + ANIMAL = 4 + }; + + inline personal_device_user_type personal_device_user_type_from_int( const unsigned int i ) { + switch (i) + { + case 0: + return personal_device_user_type::UNAVAILABLE; + case 1: + return personal_device_user_type::PEDESTRIAN; + case 2: + return personal_device_user_type::PEDALCYCLIST; + case 3: + return personal_device_user_type::PUBLIC_SAFETY_WORKER; + case 4: + return personal_device_user_type::ANIMAL; + default: + throw std::invalid_argument("Incompatible personal devvice user type value. Valid values : [0,4]"); + } + }; + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/include/serializers/sensor_data_sharing_msg_json_serializer.hpp b/streets_utils/streets_messages/include/serializers/sensor_data_sharing_msg_json_serializer.hpp new file mode 100644 index 000000000..830e34683 --- /dev/null +++ b/streets_utils/streets_messages/include/serializers/sensor_data_sharing_msg_json_serializer.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "sensor_data_sharing_msg/sensor_data_sharing_msg.hpp" +#include + + +namespace streets_utils::messages::sdsm{ + std::string to_json(const sensor_data_sharing_msg &val); + + rapidjson::Value create_timestamp(const time_stamp &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_position_3d(const position_3d &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_positional_accuracy(const positional_accuracy &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_detected_object_list(const std::vector &val, rapidjson::Document::AllocatorType &allocator ); + + rapidjson::Value create_detected_object_data(const detected_object_data &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_detected_object_data_common(const detected_object_data_common &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_detected_object_data_optional(const std::variant &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_detected_obstacle_data(const detected_obstacle_data &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_obstacle_size(const obstacle_size &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_obstacle_size_confidence(const obstacle_size_confidence &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_detected_vru_data(const detected_vru_data &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_propelled_information(const std::variant &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_detected_vehicle_data(const detected_vehicle_data &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_vehicle_attitude(const attitude &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_vehicle_attitude_confidence(const attitude_confidence &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_angular_velocity(const angular_velocity_set &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_angular_velocity_confidence(const angular_velocity_confidence_set &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_vehicle_size(const vehicle_size &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_vehicle_size_confidence(const vehicle_size_confidence &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_accelaration_set_4_way(const acceleration_set_4_way &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_position_3d(const position_offset &val, rapidjson::Document::AllocatorType &allocator); + + rapidjson::Value create_position_confidence_set(const position_confidence_set &val, rapidjson::Document::AllocatorType &allocator); +} \ No newline at end of file diff --git a/streets_utils/streets_messages/src/deserializers/detected_obj_msg_deserializer.cpp b/streets_utils/streets_messages/src/deserializers/detected_obj_msg_deserializer.cpp new file mode 100644 index 000000000..9ee3ae821 --- /dev/null +++ b/streets_utils/streets_messages/src/deserializers/detected_obj_msg_deserializer.cpp @@ -0,0 +1,121 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "deserializers/detected_obj_msg_deserializer.hpp" + +#include + +namespace streets_utils::messages::detected_objects_msg { + + detected_objects_msg from_json( const std::string &json) + { + // Deserializes the incoming json message + rapidjson::Document document = streets_utils::json_utils::parse_json(json); + detected_objects_msg msg; + + msg._type = streets_utils::json_utils::parse_string_member("type",document, true).value(); + msg._confidence = streets_utils::json_utils::parse_double_member("confidence", document, true).value(); + msg._sensor_id = streets_utils::json_utils::parse_string_member("sensorId", document, true).value(); + msg._proj_string = streets_utils::json_utils::parse_string_member("projString", document, true).value(); + msg._object_id = streets_utils::json_utils::parse_uint_member("objectId", document, true).value(); + + auto position_obj = streets_utils::json_utils::parse_object_member("position", document, true).value(); + msg._position = parse_cartesian_3d(position_obj); + + auto position_cov_obj = streets_utils::json_utils::parse_array_member("positionCovariance", document, true).value(); + msg._position_covariance = parse_covariance(position_cov_obj); + + auto velocity_obj = streets_utils::json_utils::parse_object_member("velocity", document, true).value(); + msg._velocity = parse_vector3d(velocity_obj); + + auto velocity_cov_obj = streets_utils::json_utils::parse_array_member("velocityCovariance", document, true).value(); + msg._velocity_covariance = parse_covariance(velocity_cov_obj); + + auto angular_velocity_obj = streets_utils::json_utils::parse_object_member("angularVelocity", document, true).value(); + msg._angular_velocity = parse_vector3d(angular_velocity_obj); + + auto angular_velocity_cov_obj = streets_utils::json_utils::parse_array_member("angularVelocityCovariance", document, true).value(); + msg._angular_velocity_covariance = parse_covariance(angular_velocity_cov_obj); + + auto size_obj = streets_utils::json_utils::parse_object_member("size", document, true).value(); + msg._size = parse_size(size_obj); + msg._timestamp = streets_utils::json_utils::parse_uint_member("timestamp", document, true).value(); + + + return msg; + } + + cartesian_point parse_cartesian_3d(const rapidjson::Value &val) + { + cartesian_point point; + try { + point._x = streets_utils::json_utils::parse_double_member("x", val, true).value(); + point._y = streets_utils::json_utils::parse_double_member("y", val, true).value(); + point._z = streets_utils::json_utils::parse_double_member("z", val, true).value(); + return point; + } + catch (const streets_utils::json_utils::json_parse_exception &e) { + throw streets_utils::json_utils::json_parse_exception("Parsing error occured during parsing of cartesian_3d: " + std::string(e.what())); + } + } + + vector_3d parse_vector3d(const rapidjson::Value &val) + { + vector_3d vector; + try { + vector._x = streets_utils::json_utils::parse_double_member("x", val, true).value(); + vector._y = streets_utils::json_utils::parse_double_member("y", val, true).value(); + vector._z = streets_utils::json_utils::parse_double_member("z", val, true).value(); + } + catch (const streets_utils::json_utils::json_parse_exception &e) { + throw streets_utils::json_utils::json_parse_exception("Parsing error occured during parsing of vector3d: " + std::string(e.what()) ); + } + return vector; + + } + + std::vector> parse_covariance(const rapidjson::Value::ConstArray &val) + { + // Initialize 3x3 covariance matrix + std::vector> covariance; + + for (rapidjson::SizeType i = 0; i < val.Size(); i++) + { + const rapidjson::Value& row = val[i]; + std::vector val_row; + + for (rapidjson::SizeType j = 0; j < row.Size(); j++) + { + val_row.push_back(val[i][j].GetDouble()); + } + covariance.push_back(val_row); + } + return covariance; + } + + size parse_size(const rapidjson::Value &val) + { + size size_obj; + try { + size_obj._length = streets_utils::json_utils::parse_double_member("length", val, true).value(); + size_obj._height = streets_utils::json_utils::parse_double_member("height", val, true).value(); + size_obj._width = streets_utils::json_utils::parse_double_member("width", val, true).value(); + } + catch (const streets_utils::json_utils::json_parse_exception &e) { + throw streets_utils::json_utils::json_parse_exception("Parsing error occured during parsing of size: " + std::string(e.what())); + } + return size_obj; + } + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/src/deserializers/sensor_data_sharing_msg_json_deserializer.cpp b/streets_utils/streets_messages/src/deserializers/sensor_data_sharing_msg_json_deserializer.cpp new file mode 100644 index 000000000..075410690 --- /dev/null +++ b/streets_utils/streets_messages/src/deserializers/sensor_data_sharing_msg_json_deserializer.cpp @@ -0,0 +1,301 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "deserializers/sensor_data_sharing_msg_json_deserializer.hpp" + + +namespace streets_utils::messages::sdsm { + using namespace streets_utils::json_utils; + sensor_data_sharing_msg from_json( const std::string &json ){ + rapidjson::Document document = streets_utils::json_utils::parse_json(json); + sensor_data_sharing_msg msg; + msg._msg_count = parse_uint_member("msg_cnt", document, true).value(); + msg._source_id = parse_string_member("source_id", document, true).value(); + msg._equipment_type = equipment_type_from_int(parse_uint_member("equipment_type", document, true).value()); + msg._time_stamp = parse_time_stamp(parse_object_member("sdsm_time_stamp", document, true).value()); + msg._ref_positon = parse_position_3d(parse_object_member("ref_pos", document, true).value()); + msg._ref_position_confidence = parse_positional_accuracy(parse_object_member("ref_pos_xy_conf", document, true).value()); + msg._ref_position_elevation_confidence = parse_elevation_confidence( document); + msg._objects = parse_detected_object_list(document); + return msg; + } + + time_stamp parse_time_stamp(const rapidjson::Value &val){ + time_stamp _time_stamp; + _time_stamp.offset = parse_int_member("offset", val, true).value(); + _time_stamp.second = parse_uint_member("second", val, true).value(); + _time_stamp.minute = parse_uint_member("minute", val, true).value(); + _time_stamp.hour = parse_uint_member("hour", val, true).value(); + _time_stamp.day = parse_uint_member("day", val, true).value(); + _time_stamp.month = parse_uint_member("month", val, true).value(); + _time_stamp.year = parse_uint_member("year", val, true).value(); + return _time_stamp; + + } + + position_3d parse_position_3d(const rapidjson::Value &val) { + position_3d _position_3d; + _position_3d._longitude = parse_int_member("long", val, true).value(); + _position_3d._latitude = parse_int_member("lat", val, true).value(); + // parse optional elevation + _position_3d._elevation = parse_int_member("elevation", val, false); + return _position_3d; + + } + + positional_accuracy parse_positional_accuracy(const rapidjson::Value &val){ + positional_accuracy _positional_accuracy; + _positional_accuracy._semi_major_axis_accuracy = parse_uint_member("semi_major", val, true).value(); + _positional_accuracy._semi_minor_axis_accuracy = parse_uint_member("semi_minor", val, true).value(); + _positional_accuracy._semi_major_axis_orientation = parse_uint_member("orientation",val, true).value(); + return _positional_accuracy; + + } + + std::optional parse_elevation_confidence(const rapidjson::Value &val) { + std::optional _position_confidence; + if (auto _position_confidence_value = parse_uint_member("ref_pos_el_conf", val, false); _position_confidence_value.has_value() ) { + _position_confidence = position_confidence_from_int(_position_confidence_value.value()); + } + return _position_confidence; + } + + std::vector parse_detected_object_list(const rapidjson::Value &val){ + std::vector detected_object_list; + auto json_detected_object_list = parse_array_member("objects",val, true).value(); + for (const auto &object: json_detected_object_list){ + detected_object_data data; + auto detected_object = parse_object_member("detected_object_data",object, true); + data._detected_object_common_data = parse_detected_object_data_common(parse_object_member("detected_object_common_data", detected_object.value(), true).value()); + if ( auto data_optional = parse_object_member("detected_object_optional_data", detected_object.value(), false); data_optional.has_value()) { + data._detected_object_optional_data = parse_detected_object_data_optional( data_optional.value() ); + } + detected_object_list.push_back(data); + } + return detected_object_list; + } + + detected_object_data_common parse_detected_object_data_common(const rapidjson::Value &val){ + detected_object_data_common _detected_object_common_data; + _detected_object_common_data._object_type = object_type_from_int(parse_uint_member("obj_type", val, true).value()); + _detected_object_common_data._classification_confidence = parse_uint_member("obj_type_cfd",val,true).value(); + _detected_object_common_data._object_id = parse_uint_member("object_id", val, true).value(); + _detected_object_common_data._time_measurement_offset = parse_int_member("measurement_time", val, true).value(); + _detected_object_common_data._time_confidence = time_confidence_from_int(parse_uint_member("time_confidence", val, true).value()); + _detected_object_common_data._position_offset = parse_position_offset(parse_object_member("pos" ,val, true).value()); + _detected_object_common_data._pos_confidence = parse_position_confidence_set(parse_object_member("pos_confidence", val, true).value()); + _detected_object_common_data._speed = parse_uint_member("speed", val, true).value(); + _detected_object_common_data._speed_confidence = speed_confidence_from_int(parse_uint_member("speed_confidence", val, true).value()); + // Optional + _detected_object_common_data._speed_z = parse_uint_member("speed_z", val, false); + // Optional enumeration + if ( val.HasMember("speed_z_confidence")) { + _detected_object_common_data._speed_z_confidence = speed_confidence_from_int( parse_uint_member("speed_z_confidence", val, true).value()); + } + _detected_object_common_data._heading = parse_uint_member("heading", val, true).value(); + _detected_object_common_data._heading_confidence = heading_confidence_from_int(parse_uint_member("heading_conf", val, true).value()); + if ( val.HasMember("accel_4_way") ) { + _detected_object_common_data._acceleration_4_way = parse_acceleration_4_way(parse_object_member("accel_4_way", val, false ).value()); + } + if ( val.HasMember("acc_cfd_x")) { + _detected_object_common_data._lateral_acceleration_confidence = acceleration_confidence_from_int(parse_uint_member("acc_cfd_x", val, true).value()); + } + if ( val.HasMember("acc_cfd_y")) { + _detected_object_common_data._longitudinal_acceleration_confidence = acceleration_confidence_from_int(parse_uint_member("acc_cfd_y", val, true).value()); + } + if ( val.HasMember("acc_cfd_z")) { + _detected_object_common_data._vertical_accelaration_confidence = acceleration_confidence_from_int(parse_uint_member("acc_cfd_z", val, true).value()); + } + if ( val.HasMember("acc_cfd_yaw")) { + _detected_object_common_data._yaw_rate_confidence = angular_velocity_confidence_from_int(parse_uint_member("acc_cfd_yaw", val, true).value()); + } + return _detected_object_common_data; + } + + std::optional> parse_detected_object_data_optional(const rapidjson::Value &val){ + std::optional> detected_optional_data; + if (val.HasMember("detected_vehicle_data")){ + detected_optional_data = parse_detected_vehicle_data(parse_object_member("detected_vehicle_data", val, true).value()); + } + else if (val.HasMember("detected_vru_data")) { + detected_optional_data = parse_detected_vru_data(parse_object_member("detected_vru_data", val, true).value()); + + } + else if (val.HasMember("detected_obstacle_data") ) { + detected_optional_data = parse_detected_obstacle_data(parse_object_member("detected_obstacle_data", val, true).value()); + } + return detected_optional_data; + } + + position_offset parse_position_offset(const rapidjson::Value &val) { + position_offset data; + data._offset_x = parse_int_member("offset_x", val, true).value(); + data._offset_y = parse_int_member("offset_y", val, true).value(); + data._offset_y = parse_int_member("offset_y", val, true).value(); + return data; + + } + + position_confidence_set parse_position_confidence_set(const rapidjson::Value &val) { + position_confidence_set data; + data._position_confidence = position_confidence_from_int( parse_uint_member("pos", val, true).value()); + data._elevation_confidence = position_confidence_from_int( parse_uint_member("elevation", val, true).value()); + return data; + } + + acceleration_set_4_way parse_acceleration_4_way(const rapidjson::Value &val) { + acceleration_set_4_way data; + data._lateral_accel = parse_int_member("lat", val, true).value(); + data._longitudinal_accel = parse_int_member("long", val, true).value(); + data._vertical_accel = parse_int_member("vert", val, true).value(); + data._yaw_rate = parse_int_member("yaw", val, true).value(); + return data; + } + + detected_obstacle_data parse_detected_obstacle_data(const rapidjson::Value &val){ + detected_obstacle_data data; + data._size = parse_obstacle_size(parse_object_member("obst_size", val, true).value()); + data._size_confidence = parse_obstacle_size_confidence(parse_object_member("obst_size_confidence", val, true).value()); + return data; + } + + detected_vehicle_data parse_detected_vehicle_data(const rapidjson::Value &val){ + detected_vehicle_data data; + data.exterior_lights = parse_string_member("lights", val, false); + if ( val.HasMember("veh_attitude")) { + data._veh_attitude = parse_vehicle_attitude(parse_object_member("veh_attitude", val, false).value()); + } + if ( val.HasMember("veh_attitude_confidence")) { + data._attitude_confidence = parse_vehicle_attitude_confidence(parse_object_member("veh_attitude_confidence", val, false).value()); + } + if ( val.HasMember("veh_ang_vel")) { + data._angular_velocity = parse_vehicle_angular_velocity_set(parse_object_member("veh_ang_vel", val, false).value()); + } + if ( val.HasMember("veh_ang_vel_confidence")) { + data._angular_velocity_confidence = parse_vehicle_angular_velocity_confidence_set(parse_object_member("veh_ang_vel_confidence", val, false).value()); + } + if ( val.HasMember("size")) { + data._size = parse_vehicle_size(parse_object_member("size", val, false).value()); + } + if ( val.HasMember("vehicle_size_confidence")) { + data._size_confidence = parse_vehicle_size_confidence(parse_object_member("vehicle_size_confidence", val, false).value()); + } + data._vehicle_height = parse_uint_member("height", val, false); + data._vehicle_class = parse_uint_member("vehicle_class", val, false); + data._classification_confidence = parse_uint_member("class_conf", val, false); + return data; + } + + + attitude_confidence parse_vehicle_attitude_confidence(const rapidjson::Value &val){ + attitude_confidence data; + data._pitch_confidence = heading_confidence_from_int(parse_uint_member("pitch_confidence", val, true).value()); + data._roll_confidence = heading_confidence_from_int(parse_uint_member("roll_confidence", val, true).value()); + data._yaw_confidence = heading_confidence_from_int(parse_uint_member("yaw_confidence", val, true).value()); + return data; + + } + + angular_velocity_set parse_vehicle_angular_velocity_set(const rapidjson::Value &val){ + angular_velocity_set data; + data._pitch_rate = parse_int_member("pitch_rate", val, true).value(); + data._roll_rate = parse_int_member("roll_rate", val, true).value(); + return data; + + } + + angular_velocity_confidence_set parse_vehicle_angular_velocity_confidence_set(const rapidjson::Value &val){ + angular_velocity_confidence_set data; + data._pitch_rate_confidence = angular_velocity_confidence_from_int( parse_uint_member("pitch_rate_confidence", val, true).value()); + data._roll_rate_confidence = angular_velocity_confidence_from_int( parse_uint_member("roll_rate_confidence", val, true).value()); + return data; + } + + vehicle_size parse_vehicle_size(const rapidjson::Value &val){ + vehicle_size data; + data._length = parse_uint_member("length", val, true).value(); + data._width = parse_uint_member("width", val, true).value(); + return data; + } + + vehicle_size_confidence parse_vehicle_size_confidence(const rapidjson::Value &val){ + vehicle_size_confidence data; + data._width_confidence = size_value_confidence_from_int(parse_uint_member("vehicle_width_confidence", val, true).value()); + data._length_confidence = size_value_confidence_from_int(parse_uint_member("vehicle_length_confidence", val, true).value()); + if (auto height_confidence_val = parse_uint_member("vehicle_height_confidence", val, false); height_confidence_val.has_value()) { + data._height_confidence = size_value_confidence(height_confidence_val.value()); + } + return data; + } + + attitude parse_vehicle_attitude(const rapidjson::Value &val) { + attitude data; + data._pitch = parse_int_member("pitch", val, true).value(); + data._roll = parse_int_member("roll", val, true).value(); + data._yaw = parse_int_member("yaw", val, true).value(); + return data; + } + + detected_vru_data parse_detected_vru_data(const rapidjson::Value &val){ + detected_vru_data data; + if ( auto basic_type_value = parse_uint_member("basic_type", val, false); basic_type_value.has_value() ) { + data._personal_device_user_type = personal_device_user_type_from_int( basic_type_value.value()); + } + if ( val.HasMember("propulsion") ) { + data._propulsion = parse_propelled_information(parse_object_member("propulsion", val, true).value()); + } + if (auto attachment_value = parse_uint_member("attachment", val, false); attachment_value.has_value() ){ + data._attachment = attachment_from_int( attachment_value.value()); + } + data._attachment_radius = parse_uint_member("radius", val, false); + return data; + } + + std::optional> parse_propelled_information( const rapidjson::Value &val){ + std::optional> data; + if (val.HasMember("human")) { + data = human_propelled_type_from_int(parse_uint_member("human", val, true).value()); + } + else if (val.HasMember("animal")) { + data = animal_propelled_type_from_int(parse_uint_member("animal", val, true).value()); + } + else if (val.HasMember("motor")) { + data = motorized_propelled_type_from_int(parse_uint_member("motor", val, true).value()); + } + else { + throw json_parse_exception("Propelled information requires one of the following to be define: human propelled type, animal propelled type, motorized propelled type."); + } + return data; + } + + + obstacle_size parse_obstacle_size(const rapidjson::Value &val) { + obstacle_size data; + data._length = parse_uint_member("length", val, true).value(); + data._width = parse_uint_member("width", val, true).value(); + // Optional height + data._height = parse_uint_member("height", val, true); + return data; + } + + obstacle_size_confidence parse_obstacle_size_confidence(const rapidjson::Value &val) { + obstacle_size_confidence data; + data._length_confidence = size_value_confidence_from_int( parse_uint_member("length_confidence", val, true).value()); + data._width_confidence = size_value_confidence_from_int( parse_uint_member("width_confidence", val, true).value()); + // Optional height value + if ( auto height_confidence_value = parse_uint_member("height_confidence", val, false); height_confidence_value.has_value() ){ + data._height_confidence = size_value_confidence_from_int( parse_uint_member("height_confidence", val, true).value()); + } + return data; + } +} \ No newline at end of file diff --git a/streets_utils/streets_messages/src/serializers/sensor_data_sharing_msg_json_serializer.cpp b/streets_utils/streets_messages/src/serializers/sensor_data_sharing_msg_json_serializer.cpp new file mode 100644 index 000000000..a8e1a473f --- /dev/null +++ b/streets_utils/streets_messages/src/serializers/sensor_data_sharing_msg_json_serializer.cpp @@ -0,0 +1,360 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "serializers/sensor_data_sharing_msg_json_serializer.hpp" + + +namespace streets_utils::messages::sdsm{ + std::string to_json(const sensor_data_sharing_msg &msg) { + rapidjson::Document doc; + rapidjson::Value sdsm_json(rapidjson::kObjectType); + sdsm_json.AddMember("msg_cnt", msg._msg_count, doc.GetAllocator()); + sdsm_json.AddMember("source_id", msg._source_id, doc.GetAllocator()); + sdsm_json.AddMember("equipment_type", static_cast(msg._equipment_type), doc.GetAllocator()); + // Construct SDSM Time Stamp JSON Object + auto time_stamp_json = create_timestamp(msg._time_stamp, doc.GetAllocator()); + sdsm_json.AddMember("sdsm_time_stamp", time_stamp_json, doc.GetAllocator()); + // Construct reference position JSON Object + auto position_3d_json = create_position_3d( msg._ref_positon, doc.GetAllocator() ); + sdsm_json.AddMember("ref_pos", position_3d_json, doc.GetAllocator()); + sdsm_json.AddMember("ref_pos_xy_conf", create_positional_accuracy( msg._ref_position_confidence, doc.GetAllocator()), doc.GetAllocator()); + if ( msg._ref_position_elevation_confidence.has_value() ) + sdsm_json.AddMember("ref_pos_el_conf", static_cast(msg._ref_position_elevation_confidence.value()), doc.GetAllocator()); + // Construct object list + sdsm_json.AddMember("objects", create_detected_object_list(msg._objects, doc.GetAllocator()), doc.GetAllocator()); + + rapidjson::StringBuffer buffer; + rapidjson::Writer writer(buffer); + sdsm_json.Accept(writer); + + return buffer.GetString(); + } + + rapidjson::Value create_timestamp(const time_stamp &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value time_stamp_json(rapidjson::kObjectType); + time_stamp_json.AddMember("second", val.second, allocator); + time_stamp_json.AddMember("minute", val.minute, allocator); + time_stamp_json.AddMember("hour", val.hour, allocator); + time_stamp_json.AddMember("day", val.day, allocator); + time_stamp_json.AddMember("month", val.month, allocator); + time_stamp_json.AddMember("year", val.year, allocator); + time_stamp_json.AddMember("offset", val.offset, allocator); + return time_stamp_json; + } + + rapidjson::Value create_position_3d(const position_3d &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value position_3d_json(rapidjson::kObjectType); + position_3d_json.AddMember("long", val._longitude, allocator); + position_3d_json.AddMember("lat", val._latitude, allocator); + if ( val._elevation.has_value() ) { + position_3d_json.AddMember("elevation", val._elevation.value(), allocator); + } + return position_3d_json; + } + + rapidjson::Value create_positional_accuracy(const positional_accuracy &val, rapidjson::Document::AllocatorType &allocator) { + rapidjson::Value positional_accuracy_json(rapidjson::kObjectType); + positional_accuracy_json.AddMember("semi_major", val._semi_major_axis_accuracy, allocator); + positional_accuracy_json.AddMember("semi_minor", val._semi_minor_axis_accuracy, allocator); + positional_accuracy_json.AddMember("orientation", val._semi_major_axis_orientation, allocator); + return positional_accuracy_json; + } + rapidjson::Value create_detected_object_list(const std::vector &val, rapidjson::Document::AllocatorType &allocator ){ + rapidjson::Value detected_object_list_json(rapidjson::kArrayType); + for (const auto &detected_obect : val) { + // Create and push detected object data + detected_object_list_json.PushBack(create_detected_object_data(detected_obect, allocator), allocator); + + } + return detected_object_list_json; + } + + rapidjson::Value create_detected_object_data(const detected_object_data &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value detected_object_data_json(rapidjson::kObjectType); + rapidjson::Value object_data_json(rapidjson::kObjectType); + // Create Common Data + object_data_json.AddMember("detected_object_common_data",create_detected_object_data_common(val._detected_object_common_data,allocator), allocator ); + // Create Optional Data + if ( val._detected_object_optional_data.has_value() ) + object_data_json.AddMember( + "detected_object_optional_data", + create_detected_object_data_optional(val._detected_object_optional_data.value(), allocator), + allocator); + detected_object_data_json.AddMember("detected_object_data", object_data_json , allocator); + return detected_object_data_json; + } + + + rapidjson::Value create_detected_object_data_common(const detected_object_data_common &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value detected_object_data_common_json(rapidjson::kObjectType); + detected_object_data_common_json.AddMember("obj_type", static_cast(val._object_type), allocator); + detected_object_data_common_json.AddMember("object_id", val._object_id, allocator); + detected_object_data_common_json.AddMember("obj_type_cfd", val._classification_confidence, allocator); + detected_object_data_common_json.AddMember("measurement_time", val._time_measurement_offset, allocator); + detected_object_data_common_json.AddMember("time_confidence", static_cast(val._time_confidence), allocator); + // Create Position + detected_object_data_common_json.AddMember( + "pos", + create_position_3d( val._position_offset, allocator), + allocator); + // Create Position Confidence + detected_object_data_common_json.AddMember("pos_confidence", create_position_confidence_set(val._pos_confidence, allocator), allocator ); + detected_object_data_common_json.AddMember("speed", val._speed, allocator); + detected_object_data_common_json.AddMember("speed_confidence", static_cast(val._speed_confidence), allocator); + if ( val._speed_z.has_value()) { + detected_object_data_common_json.AddMember("speed_z", val._speed_z.value(), allocator); + } + if ( val._speed_z_confidence.has_value()) { + detected_object_data_common_json.AddMember( + "speed_confidence_z", + static_cast(val._speed_z_confidence.value()), + allocator); + } + detected_object_data_common_json.AddMember("heading", val._heading, allocator); + detected_object_data_common_json.AddMember("heading_conf", static_cast(val._heading_confidence), allocator); + if (val._lateral_acceleration_confidence.has_value() ) { + detected_object_data_common_json.AddMember( + "acc_cfd_x", + static_cast(val._lateral_acceleration_confidence.value()), + allocator); + } + if (val._longitudinal_acceleration_confidence.has_value() ) { + detected_object_data_common_json.AddMember( + "acc_cfd_y", + static_cast(val._longitudinal_acceleration_confidence.value()), + allocator); + } + if (val._vertical_accelaration_confidence.has_value()){ + detected_object_data_common_json.AddMember( + "acc_cfd_z", + static_cast(val._vertical_accelaration_confidence.value()), + allocator); + } + if (val._yaw_rate_confidence.has_value()) { + detected_object_data_common_json.AddMember( + "acc_cfd_yaw", + static_cast(val._yaw_rate_confidence.value()), + allocator); + } + if (val._acceleration_4_way.has_value()) { + // Create accel_4_way + detected_object_data_common_json.AddMember( + "accel_4_way", + create_accelaration_set_4_way(val._acceleration_4_way.value(), allocator), + allocator); + } + return detected_object_data_common_json; + } + + rapidjson::Value create_accelaration_set_4_way(const acceleration_set_4_way &val, rapidjson::Document::AllocatorType &allocator) { + rapidjson::Value accelaration_set_4_way_json(rapidjson::kObjectType); + accelaration_set_4_way_json.AddMember("lat", val._lateral_accel, allocator); + accelaration_set_4_way_json.AddMember("long", val._longitudinal_accel, allocator); + accelaration_set_4_way_json.AddMember("vert", val._vertical_accel, allocator); + accelaration_set_4_way_json.AddMember("yaw", val._yaw_rate, allocator); + return accelaration_set_4_way_json; + } + + rapidjson::Value create_position_3d(const position_offset &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value position_3d_json(rapidjson::kObjectType); + position_3d_json.AddMember("offset_x", val._offset_x, allocator); + position_3d_json.AddMember("offset_y", val._offset_y, allocator); + if ( val._offset_z.has_value()) { + position_3d_json.AddMember("offset_z", val._offset_z.value(), allocator); + } + return position_3d_json; + } + + rapidjson::Value create_position_confidence_set(const position_confidence_set &val, rapidjson::Document::AllocatorType &allocator) { + rapidjson::Value position_confidence_json(rapidjson::kObjectType); + position_confidence_json.AddMember("pos",static_cast(val._position_confidence), allocator); + position_confidence_json.AddMember("elevation", static_cast(val._elevation_confidence), allocator); + return position_confidence_json; + } + rapidjson::Value create_detected_object_data_optional(const std::variant &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value detected_object_data_optional_json(rapidjson::kObjectType); + if (std::holds_alternative(val)) { + detected_object_data_optional_json.AddMember("detected_obstacle_data",create_detected_obstacle_data(std::get(val),allocator), allocator); + return detected_object_data_optional_json; + } + else if (std::holds_alternative(val)) { + detected_object_data_optional_json.AddMember("detected_vehicle_data",create_detected_vehicle_data(std::get(val),allocator), allocator); + return detected_object_data_optional_json; + } + else if (std::holds_alternative(val)) { + detected_object_data_optional_json.AddMember("detected_vru_data",create_detected_vru_data(std::get(val),allocator), allocator); + return detected_object_data_optional_json; + } + else { + throw json_utils::json_parse_exception("If present, detected optional data must include one of the following objects : detected obstacle data, detected vehicle data, detected vru data"); + } + } + + + rapidjson::Value create_detected_obstacle_data(const detected_obstacle_data &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("obst_size", create_obstacle_size(val._size, allocator), allocator); + data.AddMember("obst_size_confidence", create_obstacle_size_confidence(val._size_confidence, allocator), allocator); + return data; + } + + rapidjson::Value create_obstacle_size(const obstacle_size &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("width", val._width, allocator); + data.AddMember("length", val._length, allocator); + // Optional Height + if (val._height.has_value() ) { + data.AddMember("height", val._height.value(), allocator); + } + + return data; + } + + rapidjson::Value create_obstacle_size_confidence(const obstacle_size_confidence &val, rapidjson::Document::AllocatorType &allocator) { + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("width_confidence", static_cast(val._width_confidence), allocator); + data.AddMember("length_confidence", static_cast(val._length_confidence), allocator); + // Optional height + if ( val._height_confidence.has_value()) { + data.AddMember("height_confidence", static_cast(val._height_confidence.value()), allocator); + } + return data; + } + + rapidjson::Value create_detected_vru_data(const detected_vru_data &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + if (val._personal_device_user_type.has_value() ) { + data.AddMember("basic_type", static_cast(val._personal_device_user_type.value()), allocator); + } + if (val._attachment.has_value() ) { + data.AddMember("attachment", static_cast(val._attachment.value()), allocator); + } + if (val._propulsion.has_value() ) { + data.AddMember("propulsion", create_propelled_information(val._propulsion.value(), allocator), allocator); + } + if (val._attachment_radius.has_value() ) { + data.AddMember("radius", val._attachment_radius.value(), allocator); + } + return data; + } + + rapidjson::Value create_detected_vehicle_data(const detected_vehicle_data &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + if ( val.exterior_lights.has_value() ) { + data.AddMember("lights", val.exterior_lights.value(), allocator); + } + if ( val._veh_attitude.has_value() ) { + data.AddMember("veh_attitude", create_vehicle_attitude(val._veh_attitude.value(), allocator), allocator); + } + if ( val._attitude_confidence.has_value() ) { + data.AddMember("veh_attitude_confidence", create_vehicle_attitude_confidence(val._attitude_confidence.value(), allocator), allocator); + } + if ( val._angular_velocity.has_value() ) { + data.AddMember("veh_ang_vel", create_angular_velocity(val._angular_velocity.value(), allocator), allocator); + } + if ( val._angular_velocity_confidence.has_value() ){ + data.AddMember("veh_ang_vel_confidence", create_angular_velocity_confidence(val._angular_velocity_confidence.value(),allocator), allocator); + } + if ( val._size.has_value() ) { + data.AddMember("size", create_vehicle_size(val._size.value(), allocator), allocator); + } + if ( val._size_confidence.has_value() ) { + data.AddMember("vehicle_size_confidence", create_vehicle_size_confidence(val._size_confidence.value(), allocator), allocator); + } + if (val._vehicle_height.has_value() ) { + data.AddMember("height", val._vehicle_height.value(), allocator); + } + if (val._vehicle_class.has_value() ) { + data.AddMember("vehicle_class", val._vehicle_class.value(), allocator); + } + if ( val._classification_confidence.has_value() ) { + data.AddMember("vehicle_class_conf", val._classification_confidence.value(), allocator); + } + return data; + } + + + + rapidjson::Value create_propelled_information(const std::variant &val, rapidjson::Document::AllocatorType &allocator) { + rapidjson::Value data(rapidjson::kObjectType); + if ( std::holds_alternative(val) ) { + data.AddMember("human", static_cast( std::get(val)), allocator); + return data; + } + else if ( std::holds_alternative(val) ) { + data.AddMember("animal", static_cast( std::get(val)), allocator); + return data; + } + else if ( std::holds_alternative(val) ) { + data.AddMember("motor", static_cast( std::get(val)), allocator); + return data; + } + else { + throw json_utils::json_parse_exception("If present, propelled infromation must include one of the following objects : animal propelled type, motorized propelled type, human propelled type"); + } + } + + + rapidjson::Value create_vehicle_attitude(const attitude &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("pitch", val._pitch, allocator); + data.AddMember("roll", val._roll, allocator); + data.AddMember("yaw", val._yaw, allocator); + return data; + } + + rapidjson::Value create_vehicle_attitude_confidence(const attitude_confidence &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("pitch_confidence", static_cast(val._pitch_confidence), allocator); + data.AddMember("roll_confidence", static_cast(val._roll_confidence), allocator); + data.AddMember("yaw_confidence", static_cast(val._yaw_confidence), allocator); + return data; + } + + rapidjson::Value create_angular_velocity(const angular_velocity_set &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("pitch_rate", static_cast(val._pitch_rate), allocator); + data.AddMember("roll_rate", static_cast(val._roll_rate), allocator); + return data; + } + + rapidjson::Value create_angular_velocity_confidence(const angular_velocity_confidence_set &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + if ( val._pitch_rate_confidence.has_value() ) { + data.AddMember("pitch_rate_confidence", static_cast(val._pitch_rate_confidence.value()), allocator); + } + if ( val._roll_rate_confidence.has_value() ) { + data.AddMember("roll_rate_confidence", static_cast(val._roll_rate_confidence.value()), allocator); + } + return data; + } + + rapidjson::Value create_vehicle_size(const vehicle_size &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("width", val._width, allocator); + data.AddMember("length", val._length, allocator); + return data; + } + + rapidjson::Value create_vehicle_size_confidence(const vehicle_size_confidence &val, rapidjson::Document::AllocatorType &allocator){ + rapidjson::Value data(rapidjson::kObjectType); + data.AddMember("vehicle_width_confidence", static_cast(val._width_confidence), allocator); + data.AddMember("vehicle_length_confidence", static_cast(val._length_confidence), allocator); + if ( val._height_confidence.has_value() ) { + data.AddMember("vehicle_height_confidence", static_cast(val._height_confidence.value()), allocator); + } + return data; + } + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/deserializers/detected_obj_msg_deserialization_test.cpp b/streets_utils/streets_messages/test/deserializers/detected_obj_msg_deserialization_test.cpp new file mode 100644 index 000000000..675054ed3 --- /dev/null +++ b/streets_utils/streets_messages/test/deserializers/detected_obj_msg_deserialization_test.cpp @@ -0,0 +1,107 @@ +// Copyright 2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include + +TEST(detected_obj_msg_deserializer_test, deserialize) +{ + std::string json_prediction = R"( + { + "type":"CAR", + "confidence":0.7, + "sensorId":"sensor1", + "projString":"projectionString2", + "objectId":27, + "position":{ + "x":-1.1, + "y":-2.0, + "z":-3.2 + }, + "positionCovariance":[[1.0,0.0,0.0],[1.0,0.0,0.0],[1.0,0.0,0.0]], + "velocity":{ + "x":1.0, + "y":1.0, + "z":1.0 + }, + "velocityCovariance":[[1.0,0.0,0.0],[1.0,0.0,0.0],[1.0,0.0,0.0]], + "angularVelocity":{ + "x":0.1, + "y":0.2, + "z":0.3 + }, + "angularVelocityCovariance":[[1.0,0.0,0.0],[1.0,0.0,0.0],[1.0,0.0,0.0]], + "size":{ + "length":2.0, + "height":1.0, + "width":0.5 + }, + "timestamp":1702335309 + } + )"; + + auto msg = streets_utils::messages::detected_objects_msg::from_json(json_prediction); + EXPECT_EQ(msg._type, "CAR"); + EXPECT_NEAR(0.7,msg._confidence, 0.01); + EXPECT_EQ(msg._sensor_id, "sensor1"); + EXPECT_EQ(msg._object_id, 27); + EXPECT_NEAR(msg._position._x, -1.1, 0.001); + EXPECT_NEAR(msg._position._y, -2.0, 0.001); + EXPECT_NEAR(msg._position._z, -3.2, 0.001); + // Position Covariance + std::vector> pose_cov = {{1,0,0},{1,0,0},{1,0,0}}; + for (size_t i = 0 ; i < msg._position_covariance.size();i++) + { + for (size_t j=0; j< msg._position_covariance[i].size();j++) + { + EXPECT_NEAR(msg._position_covariance[i][j], pose_cov[i][j], 0.001); + } + + } + + EXPECT_NEAR(msg._velocity._x, 1.0, 0.001); + EXPECT_NEAR(msg._velocity._y, 1.0, 0.001); + EXPECT_NEAR(msg._velocity._z,1.0, 0.001); + // Velocity Covariance + std::vector> vel_cov = {{1,0,0},{1,0,0},{1,0,0}}; + for (size_t i = 0 ; i < msg._velocity_covariance.size();i++) + { + for (size_t j=0; j< msg._velocity_covariance[i].size();j++) + { + EXPECT_NEAR(msg._velocity_covariance[i][j], vel_cov[i][j], 0.001); + } + + } + EXPECT_NEAR(msg._angular_velocity._x,0.1, 0.001); + EXPECT_NEAR(msg._angular_velocity._y,0.2, 0.001); + EXPECT_NEAR(msg._angular_velocity._z,0.3, 0.001); + // AngularVelocity Covariance + std::vector> ang_vel_cov = {{1,0,0},{1,0,0},{1,0,0}}; + for (size_t i = 0 ; i < msg._angular_velocity_covariance.size();i++) + { + for (size_t j=0; j< msg._angular_velocity_covariance[i].size();j++) + { + EXPECT_NEAR(msg._angular_velocity_covariance[i][j], ang_vel_cov[i][j], 0.001); + } + + } + EXPECT_NEAR(msg._size._length,2.0, 0.001); + EXPECT_NEAR(msg._size._height,1.0, 0.001); + EXPECT_NEAR(msg._size._width,0.5, 0.001); + EXPECT_EQ(msg._timestamp, 1702335309); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/deserializers/sensor_data_sharing_msg_json_deserialization_test.cpp b/streets_utils/streets_messages/test/deserializers/sensor_data_sharing_msg_json_deserialization_test.cpp new file mode 100644 index 000000000..b3ac04608 --- /dev/null +++ b/streets_utils/streets_messages/test/deserializers/sensor_data_sharing_msg_json_deserialization_test.cpp @@ -0,0 +1,463 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include +using namespace streets_utils::messages::sdsm; + +TEST(sensor_dara_sharing_msg_json_deserialization_test, deserialize) { + + std::string json = "{" + "\"equipment_type\": 0," + "\"msg_cnt\": 255," + "\"ref_pos\": {" + " \"long\": 1800000001," + " \"elevation\": 30," + " \"lat\": 900000001" + "}," + "\"ref_pos_el_conf\": 10," + "\"ref_pos_xy_conf\": {" + " \"orientation\": 25000," + " \"semi_major\": 235," + " \"semi_minor\": 200" + "}," + "\"sdsm_time_stamp\": {" + " \"day\": 4," + " \"hour\": 19," + " \"minute\": 15," + " \"month\": 7," + " \"offset\": 400," + " \"second\": 5000," + " \"year\": 2007" + "}," + "\"source_id\": \"0102C304\"," + "\"objects\": [" + " {" + " \"detected_object_data\": {" + " \"detected_object_common_data\": {" + " \"acc_cfd_x\": 4," + " \"acc_cfd_y\": 5," + " \"acc_cfd_yaw\": 3," + " \"acc_cfd_z\": 6," + " \"accel_4_way\": {" + " \"lat\": -500," + " \"long\": 200," + " \"vert\": 1," + " \"yaw\": 400" + " }," + " \"heading\": 16000," + " \"heading_conf\": 4," + " \"measurement_time\": -1100," + " \"object_id\": 12200," + " \"obj_type\": 1," + " \"obj_type_cfd\": 65," + " \"pos\": {" + " \"offset_x\": 4000," + " \"offset_y\": -720," + " \"offset_z\": 20" + " }," + " \"pos_confidence\": {" + " \"elevation\": 5," + " \"pos\": 2" + " }," + " \"speed\": 2100," + " \"speed_confidence\": 3," + " \"speed_confidence_z\": 4," + " \"speed_z\": 1000," + " \"time_confidence\": 2" + " }" + " }" + " }" + " ]" + "}"; + auto msg = from_json(json ); + + EXPECT_EQ(255, msg._msg_count); + EXPECT_EQ("0102C304", msg._source_id); + EXPECT_EQ(equipment_type::UNKNOWN, msg._equipment_type); + // Confirm timestamp + EXPECT_EQ(400, msg._time_stamp.offset); + EXPECT_EQ(5000, msg._time_stamp.second); + EXPECT_EQ(15, msg._time_stamp.minute); + EXPECT_EQ(19, msg._time_stamp.hour); + EXPECT_EQ(4, msg._time_stamp.day); + EXPECT_EQ(7, msg._time_stamp.month); + EXPECT_EQ(2007, msg._time_stamp.year); + // Confirm reference position + EXPECT_EQ(1800000001,msg._ref_positon._longitude); + EXPECT_EQ(900000001, msg._ref_positon._latitude ); + // Confirm optional elevation is present + EXPECT_TRUE(msg._ref_positon._elevation.has_value()); + EXPECT_EQ(30, msg._ref_positon._elevation); + // Confirm positional accuracy + EXPECT_EQ(235, msg._ref_position_confidence._semi_major_axis_accuracy); + EXPECT_EQ(200, msg._ref_position_confidence._semi_minor_axis_accuracy); + EXPECT_EQ(25000, msg._ref_position_confidence._semi_major_axis_orientation); + // Get Detection + ASSERT_EQ(1, msg._objects.size()); + auto detection = msg._objects[0]; + // Confirm Common Data + EXPECT_EQ(12200 ,detection._detected_object_common_data._object_id); + EXPECT_EQ(object_type_from_int(1) ,detection._detected_object_common_data._object_type); + EXPECT_EQ(65,detection._detected_object_common_data._classification_confidence); + // Confirm 4 way accel + EXPECT_EQ( -500, detection._detected_object_common_data._acceleration_4_way->_lateral_accel); + EXPECT_EQ( 200, detection._detected_object_common_data._acceleration_4_way->_longitudinal_accel); + EXPECT_EQ( 1, detection._detected_object_common_data._acceleration_4_way->_vertical_accel); + EXPECT_EQ( 400, detection._detected_object_common_data._acceleration_4_way->_yaw_rate); + // Confirm acceleration confidence + EXPECT_EQ( acceleration_confidence_from_int(4), detection._detected_object_common_data._lateral_acceleration_confidence); + EXPECT_EQ( acceleration_confidence_from_int(5), detection._detected_object_common_data._longitudinal_acceleration_confidence); + EXPECT_EQ( acceleration_confidence_from_int(6), detection._detected_object_common_data._vertical_accelaration_confidence); + EXPECT_EQ( angular_velocity_confidence_from_int(3), detection._detected_object_common_data._yaw_rate_confidence); + // Confirm position + + + +} + +TEST(sensor_dara_sharing_msg_json_deserialization_test, deserialize_optional_obstacle_data) { + + std::string json = "{" + " \"equipment_type\": 1," + " \"msg_cnt\": 1," + " \"objects\": [" + " {" + " \"detected_object_data\": {" + " \"detected_object_common_data\": {" + " \"acc_cfd_x\": 4," + " \"acc_cfd_y\": 5," + " \"acc_cfd_yaw\": 3," + " \"acc_cfd_z\": 6," + " \"accel_4_way\": {" + " \"lat\": -500," + " \"long\": 200," + " \"vert\": 1," + " \"yaw\": 400" + " }," + " \"heading\": 16000," + " \"heading_conf\": 4," + " \"measurement_time\": -1100," + " \"object_id\": 12200," + " \"obj_type\": 1," + " \"obj_type_cfd\": 65," + " \"pos\": {" + " \"offset_x\": 4000," + " \"offset_y\": -720," + " \"offset_z\": 20" + " }," + " \"pos_confidence\": {" + " \"elevation\": 5," + " \"pos\": 2" + " }," + " \"speed\": 2100," + " \"speed_confidence\": 3," + " \"speed_confidence_z\": 4," + " \"speed_z\": 1000," + " \"time_confidence\": 2" + " }," + " \"detected_object_optional_data\": {" + " \"detected_obstacle_data\": {" + " \"obst_size\": {" + " \"height\": 100," + " \"length\": 300," + " \"width\": 400" + " }," + " \"obst_size_confidence\": {" + " \"height_confidence\": 8," + " \"length_confidence\": 7," + " \"width_confidence\": 6" + " }" + " }" + " }" + " }" + " }" + " ]," + " \"ref_pos\": {" + " \"long\": 600000000," + " \"elevation\": 30," + " \"lat\": 400000000" + " }," + " \"ref_pos_el_conf\": 10," + " \"ref_pos_xy_conf\": {" + " \"orientation\": 25000," + " \"semi_major\": 235," + " \"semi_minor\": 200" + " }," + " \"sdsm_time_stamp\": {" + " \"day\": 4," + " \"hour\": 19," + " \"minute\": 15," + " \"month\": 7," + " \"offset\": 400," + " \"second\": 5000," + " \"year\": 2007" + " }," + " \"source_id\": \"0102C304\"" + " }"; + auto msg = from_json(json ); + + EXPECT_EQ(1, msg._msg_count); + EXPECT_EQ("0102C304", msg._source_id); + EXPECT_EQ(equipment_type::RSU, msg._equipment_type); + // Confirm timestamp + EXPECT_EQ(400, msg._time_stamp.offset); + EXPECT_EQ(5000, msg._time_stamp.second); + EXPECT_EQ(15, msg._time_stamp.minute); + EXPECT_EQ(19, msg._time_stamp.hour); + EXPECT_EQ(4, msg._time_stamp.day); + EXPECT_EQ(7, msg._time_stamp.month); + EXPECT_EQ(2007, msg._time_stamp.year); + // Confirm reference position + EXPECT_EQ(600000000,msg._ref_positon._longitude); + EXPECT_EQ(400000000, msg._ref_positon._latitude ); + // Confirm optional elevation is present + EXPECT_TRUE(msg._ref_positon._elevation.has_value()); + EXPECT_EQ(30, msg._ref_positon._elevation); + // Confirm positional accuracy + EXPECT_EQ(235, msg._ref_position_confidence._semi_major_axis_accuracy); + EXPECT_EQ(200, msg._ref_position_confidence._semi_minor_axis_accuracy); + EXPECT_EQ(25000, msg._ref_position_confidence._semi_major_axis_orientation); + + +} + +TEST(sensor_dara_sharing_msg_json_deserialization_test, deserialize_optional_vru_data) { + + std::string json = "{" + " \"equipment_type\": 1," + " \"msg_cnt\": 1," + " \"objects\": [" + " {" + " \"detected_object_data\": {" + " \"detected_object_common_data\": {" + " \"acc_cfd_x\": 4," + " \"acc_cfd_y\": 5," + " \"acc_cfd_yaw\": 3," + " \"acc_cfd_z\": 6," + " \"accel_4_way\": {" + " \"lat\": -500," + " \"long\": 200," + " \"vert\": 1," + " \"yaw\": 400" + " }," + " \"heading\": 16000," + " \"heading_conf\": 4," + " \"measurement_time\": -1100," + " \"object_id\": 12200," + " \"obj_type\": 1," + " \"obj_type_cfd\": 65," + " \"pos\": {" + " \"offset_x\": 4000," + " \"offset_y\": -720," + " \"offset_z\": 20" + " }," + " \"pos_confidence\": {" + " \"elevation\": 5," + " \"pos\": 2" + " }," + " \"speed\": 2100," + " \"speed_confidence\": 3," + " \"speed_confidence_z\": 4," + " \"speed_z\": 1000," + " \"time_confidence\": 2" + " }," + " \"detected_object_optional_data\": {" + " \"detected_vru_data\": {" + " \"attachment\": 3," + " \"basic_type\": 1," + " \"propulsion\": {" + " \"human\": 2" + " }," + " \"radius\": 30" + " }" + " }" + " }" + " }" + " ]," + " \"ref_pos\": {" + " \"long\": 600000000," + " \"elevation\": 30," + " \"lat\": 400000000" + " }," + " \"ref_pos_el_conf\": 10," + " \"ref_pos_xy_conf\": {" + " \"orientation\": 25000," + " \"semi_major\": 235," + " \"semi_minor\": 200" + " }," + " \"sdsm_time_stamp\": {" + " \"day\": 4," + " \"hour\": 19," + " \"minute\": 15," + " \"month\": 7," + " \"offset\": 400," + " \"second\": 5000," + " \"year\": 2007" + " }," + " \"source_id\": \"0102C304\"" + " }"; + auto msg = from_json(json ); + + EXPECT_EQ(1, msg._msg_count); + EXPECT_EQ("0102C304", msg._source_id); + EXPECT_EQ(equipment_type::RSU, msg._equipment_type); + // Confirm timestamp + EXPECT_EQ(400, msg._time_stamp.offset); + EXPECT_EQ(5000, msg._time_stamp.second); + EXPECT_EQ(15, msg._time_stamp.minute); + EXPECT_EQ(19, msg._time_stamp.hour); + EXPECT_EQ(4, msg._time_stamp.day); + EXPECT_EQ(7, msg._time_stamp.month); + EXPECT_EQ(2007, msg._time_stamp.year); + // Confirm reference position + EXPECT_EQ(600000000,msg._ref_positon._longitude); + EXPECT_EQ(400000000, msg._ref_positon._latitude ); + // Confirm optional elevation is present + EXPECT_TRUE(msg._ref_positon._elevation.has_value()); + EXPECT_EQ(30, msg._ref_positon._elevation); + // Confirm positional accuracy + EXPECT_EQ(235, msg._ref_position_confidence._semi_major_axis_accuracy); + EXPECT_EQ(200, msg._ref_position_confidence._semi_minor_axis_accuracy); + EXPECT_EQ(25000, msg._ref_position_confidence._semi_major_axis_orientation); + + +} + +TEST(sensor_dara_sharing_msg_json_deserialization_test, deserialize_optional_vehicle_data) { + + std::string json = "{" + " \"equipment_type\": 1," + " \"msg_cnt\": 1," + " \"objects\": [" + " {" + " \"detected_object_data\": {" + " \"detected_object_common_data\": {" + " \"acc_cfd_x\": 4," + " \"acc_cfd_y\": 5," + " \"acc_cfd_yaw\": 3," + " \"acc_cfd_z\": 6," + " \"accel_4_way\": {" + " \"lat\": -500," + " \"long\": 200," + " \"vert\": 1," + " \"yaw\": 400" + " }," + " \"heading\": 16000," + " \"heading_conf\": 4," + " \"measurement_time\": -1100," + " \"object_id\": 12200," + " \"obj_type\": 1," + " \"obj_type_cfd\": 65," + " \"pos\": {" + " \"offset_x\": 4000," + " \"offset_y\": -720," + " \"offset_z\": 20" + " }," + " \"pos_confidence\": {" + " \"elevation\": 5," + " \"pos\": 2" + " }," + " \"speed\": 2100," + " \"speed_confidence\": 3," + " \"speed_confidence_z\": 4," + " \"speed_z\": 1000," + " \"time_confidence\": 2" + " }," + " \"detected_object_optional_data\": {" + " \"detected_vehicle_data\": {" + " \"height\": 70," + " \"lights\": 8," + " \"size\": {" + " \"length\": 700," + " \"width\": 300" + " }," + " \"veh_ang_vel\": {" + " \"pitch_rate\": 600," + " \"roll_rate\": -800" + " }," + " \"veh_ang_vel_confidence\": {" + " \"pitch_rate_confidence\": 3," + " \"roll_rate_confidence\": 4" + " }," + " \"veh_attitude\": {" + " \"pitch\": 2400," + " \"roll\": -12000," + " \"yaw\": 400" + " }," + " \"veh_attitude_confidence\": {" + " \"pitch_confidence\": 2," + " \"roll_confidence\": 3," + " \"yaw_confidence\": 4" + " }," + " \"vehicle_class\": 11," + " \"vehicle_class_conf\": 75," + " \"vehicle_size_confidence\": {" + " \"vehicle_height_confidence\": 5," + " \"vehicle_length_confidence\": 6," + " \"vehicle_width_confidence\": 7" + " }" + " }" + " }" + " }" + " }" + " ]," + " \"ref_pos\": {" + " \"long\": 600000000," + " \"elevation\": 30," + " \"lat\": 400000000" + " }," + " \"ref_pos_el_conf\": 10," + " \"ref_pos_xy_conf\": {" + " \"orientation\": 25000," + " \"semi_major\": 235," + " \"semi_minor\": 200" + " }," + " \"sdsm_time_stamp\": {" + " \"day\": 4," + " \"hour\": 19," + " \"minute\": 15," + " \"month\": 7," + " \"offset\": 400," + " \"second\": 5000," + " \"year\": 2007" + " }," + " \"source_id\": \"0102C304\"" + " }"; + auto msg = from_json(json ); + + EXPECT_EQ(1, msg._msg_count); + EXPECT_EQ("0102C304", msg._source_id); + EXPECT_EQ(equipment_type::RSU, msg._equipment_type); + // Confirm timestamp + EXPECT_EQ(400, msg._time_stamp.offset); + EXPECT_EQ(5000, msg._time_stamp.second); + EXPECT_EQ(15, msg._time_stamp.minute); + EXPECT_EQ(19, msg._time_stamp.hour); + EXPECT_EQ(4, msg._time_stamp.day); + EXPECT_EQ(7, msg._time_stamp.month); + EXPECT_EQ(2007, msg._time_stamp.year); + // Confirm reference position + EXPECT_EQ(600000000,msg._ref_positon._longitude); + EXPECT_EQ(400000000, msg._ref_positon._latitude ); + // Confirm optional elevation is present + EXPECT_TRUE(msg._ref_positon._elevation.has_value()); + EXPECT_EQ(30, msg._ref_positon._elevation); + // Confirm positional accuracy + EXPECT_EQ(235, msg._ref_position_confidence._semi_major_axis_accuracy); + EXPECT_EQ(200, msg._ref_position_confidence._semi_minor_axis_accuracy); + EXPECT_EQ(25000, msg._ref_position_confidence._semi_major_axis_orientation); + + +} diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/acceleration_confidence_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/acceleration_confidence_test.cpp new file mode 100644 index 000000000..2549b95ab --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/acceleration_confidence_test.cpp @@ -0,0 +1,31 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(acceleration_confidence_test, test_from_int){ + EXPECT_EQ(acceleration_confidence::UNAVAILABLE, acceleration_confidence_from_int(0)); + EXPECT_EQ(acceleration_confidence::ACCL_100, acceleration_confidence_from_int(1)); + EXPECT_EQ(acceleration_confidence::ACCL_10, acceleration_confidence_from_int(2)); + EXPECT_EQ(acceleration_confidence::ACCL_5, acceleration_confidence_from_int(3)); + EXPECT_EQ(acceleration_confidence::ACCL_1, acceleration_confidence_from_int(4)); + EXPECT_EQ(acceleration_confidence::ACCL_0_1, acceleration_confidence_from_int(5)); + EXPECT_EQ(acceleration_confidence::ACCL_0_05, acceleration_confidence_from_int(6)); + EXPECT_EQ(acceleration_confidence::ACCL_0_01, acceleration_confidence_from_int(7)); + // Value outside of range set to unavailable + EXPECT_THROW( acceleration_confidence_from_int(8), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/angular_velocity_confidence_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/angular_velocity_confidence_test.cpp new file mode 100644 index 000000000..3ff05a9d9 --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/angular_velocity_confidence_test.cpp @@ -0,0 +1,30 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(angular_velocity_confidence_test, test_from_int){ + EXPECT_EQ(angular_velocity_confidence::UNAVAILABLE, angular_velocity_confidence_from_int(0)); + EXPECT_EQ(angular_velocity_confidence::DEGSEC_100, angular_velocity_confidence_from_int(1)); + EXPECT_EQ(angular_velocity_confidence::DEGSEC_10, angular_velocity_confidence_from_int(2)); + EXPECT_EQ(angular_velocity_confidence::DEGSEC_05, angular_velocity_confidence_from_int(3)); + EXPECT_EQ(angular_velocity_confidence::DEGSEC_01, angular_velocity_confidence_from_int(4)); + EXPECT_EQ(angular_velocity_confidence::DEGSEC_0_1, angular_velocity_confidence_from_int(5)); + EXPECT_EQ(angular_velocity_confidence::DEGSEC_0_05, angular_velocity_confidence_from_int(6)); + EXPECT_EQ(angular_velocity_confidence::DEGSEC_0_01, angular_velocity_confidence_from_int(7)); + EXPECT_THROW( angular_velocity_confidence_from_int(8), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/animal_propelled_type_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/animal_propelled_type_test.cpp new file mode 100644 index 000000000..ae4101c3a --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/animal_propelled_type_test.cpp @@ -0,0 +1,28 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(animal_propelled_type_test, test_from_int){ + EXPECT_EQ(animal_propelled_type::UNAVAILABLE, animal_propelled_type_from_int(0)); + EXPECT_EQ(animal_propelled_type::OTHER_TYPES, animal_propelled_type_from_int(1)); + EXPECT_EQ(animal_propelled_type::ANIMAL_MOUNTED, animal_propelled_type_from_int(2)); + EXPECT_EQ(animal_propelled_type::ANIMAL_DRAWN_CARRIAGE, animal_propelled_type_from_int(3)); + // Value outside of range set to unavailable + EXPECT_THROW( animal_propelled_type_from_int(4), std::invalid_argument); + + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/attachment_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/attachment_test.cpp new file mode 100644 index 000000000..eaaf6cd81 --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/attachment_test.cpp @@ -0,0 +1,29 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(attachment_test, test_from_int){ + EXPECT_EQ(attachment::UNAVAILABLE, attachment_from_int(0)); + EXPECT_EQ(attachment::STROLLER, attachment_from_int(1)); + EXPECT_EQ(attachment::BICYLE_TRAILER, attachment_from_int(2)); + EXPECT_EQ(attachment::CART, attachment_from_int(3)); + EXPECT_EQ(attachment::WHEEL_CHAIR, attachment_from_int(4)); + EXPECT_EQ(attachment::OTHER_WALK_ASSIST_ATTACHMENTS, attachment_from_int(5)); + EXPECT_EQ(attachment::PET, attachment_from_int(6)); + EXPECT_THROW( attachment_from_int(7), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/equipment_type_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/equipment_type_test.cpp new file mode 100644 index 000000000..963459aca --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/equipment_type_test.cpp @@ -0,0 +1,26 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(equipment_type_test, test_from_int){ + EXPECT_EQ(equipment_type::UNKNOWN, equipment_type_from_int(0)); + EXPECT_EQ(equipment_type::RSU, equipment_type_from_int(1)); + EXPECT_EQ(equipment_type::OBU, equipment_type_from_int(2)); + EXPECT_EQ(equipment_type::VRU, equipment_type_from_int(3)); + EXPECT_THROW( equipment_type_from_int(4), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/heading_confidence_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/heading_confidence_test.cpp new file mode 100644 index 000000000..82254e1f1 --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/heading_confidence_test.cpp @@ -0,0 +1,31 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(heading_confidence_test, test_from_int){ + EXPECT_EQ(heading_confidence::UNAVAILABLE, heading_confidence_from_int(0)); + EXPECT_EQ(heading_confidence::PREC_10_deg, heading_confidence_from_int(1)); + EXPECT_EQ(heading_confidence::PREC_05_deg, heading_confidence_from_int(2)); + EXPECT_EQ(heading_confidence::PREC_01_deg, heading_confidence_from_int(3)); + EXPECT_EQ(heading_confidence::PREC_0_1_deg, heading_confidence_from_int(4)); + EXPECT_EQ(heading_confidence::PREC_0_05_deg, heading_confidence_from_int(5)); + EXPECT_EQ(heading_confidence::PREC_0_01_deg, heading_confidence_from_int(6)); + EXPECT_EQ(heading_confidence::PREC_0_0125_deg, heading_confidence_from_int(7)); + // Value outside of range set to unavailable + EXPECT_THROW( heading_confidence_from_int(8), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/human_propelled_type_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/human_propelled_type_test.cpp new file mode 100644 index 000000000..f90ac943a --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/human_propelled_type_test.cpp @@ -0,0 +1,28 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(human_propelled_type_test, test_from_int){ + EXPECT_EQ(human_propelled_type::UNAVAILABLE, human_propelled_type_from_int(0)); + EXPECT_EQ(human_propelled_type::OTHER_TYPES, human_propelled_type_from_int(1)); + EXPECT_EQ(human_propelled_type::ON_FOOT, human_propelled_type_from_int(2)); + EXPECT_EQ(human_propelled_type::SKATEBOARD, human_propelled_type_from_int(3)); + EXPECT_EQ(human_propelled_type::PUSH_OR_KICK_SCOOTER, human_propelled_type_from_int(4)); + EXPECT_EQ(human_propelled_type::WHEELCHAIR, human_propelled_type_from_int(5)); + EXPECT_THROW( human_propelled_type_from_int(6), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/motorized_propelled_type_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/motorized_propelled_type_test.cpp new file mode 100644 index 000000000..de899dadc --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/motorized_propelled_type_test.cpp @@ -0,0 +1,28 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(motorized_propelled_type_test, test_from_int){ + EXPECT_EQ(motorized_propelled_type::UNAVAILABLE, motorized_propelled_type_from_int(0)); + EXPECT_EQ(motorized_propelled_type::OTHER_TYPES, motorized_propelled_type_from_int(1)); + EXPECT_EQ(motorized_propelled_type::WHEEL_CHAIR, motorized_propelled_type_from_int(2)); + EXPECT_EQ(motorized_propelled_type::BICYCLE, motorized_propelled_type_from_int(3)); + EXPECT_EQ(motorized_propelled_type::SCOOTER, motorized_propelled_type_from_int(4)); + EXPECT_EQ(motorized_propelled_type::SELF_BALANCING_DEVICE, motorized_propelled_type_from_int(5)); + EXPECT_THROW( motorized_propelled_type_from_int(6), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/object_type_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/object_type_test.cpp new file mode 100644 index 000000000..82aa28884 --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/object_type_test.cpp @@ -0,0 +1,26 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(object_type_test, test_from_int){ + EXPECT_EQ(object_type::UNKNOWN, object_type_from_int(0)); + EXPECT_EQ(object_type::VEHICLE, object_type_from_int(1)); + EXPECT_EQ(object_type::VRU, object_type_from_int(2)); + EXPECT_EQ(object_type::ANIMAL, object_type_from_int(3)); + EXPECT_THROW( object_type_from_int(4), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/personal_device_user_type.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/personal_device_user_type.cpp new file mode 100644 index 000000000..fcdadc3d4 --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/personal_device_user_type.cpp @@ -0,0 +1,27 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(personal_device_user_type_test, test_from_int){ + EXPECT_EQ(personal_device_user_type::UNAVAILABLE, personal_device_user_type_from_int(0)); + EXPECT_EQ(personal_device_user_type::PEDESTRIAN, personal_device_user_type_from_int(1)); + EXPECT_EQ(personal_device_user_type::PEDALCYCLIST, personal_device_user_type_from_int(2)); + EXPECT_EQ(personal_device_user_type::PUBLIC_SAFETY_WORKER, personal_device_user_type_from_int(3)); + EXPECT_EQ(personal_device_user_type::ANIMAL, personal_device_user_type_from_int(4)); + EXPECT_THROW( personal_device_user_type_from_int(5), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/position_confidence_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/position_confidence_test.cpp new file mode 100644 index 000000000..e76f6c5ec --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/position_confidence_test.cpp @@ -0,0 +1,38 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(position_confidence_test, test_from_int){ + EXPECT_EQ(position_confidence::UNAVAILABLE, position_confidence_from_int(0)); + EXPECT_EQ(position_confidence::A_500M, position_confidence_from_int(1)); + EXPECT_EQ(position_confidence::A_200M, position_confidence_from_int(2)); + EXPECT_EQ(position_confidence::A_100M, position_confidence_from_int(3)); + EXPECT_EQ(position_confidence::A_50M, position_confidence_from_int(4)); + EXPECT_EQ(position_confidence::A_20M, position_confidence_from_int(5)); + EXPECT_EQ(position_confidence::A_10M, position_confidence_from_int(6)); + EXPECT_EQ(position_confidence::A_5M, position_confidence_from_int(7)); + EXPECT_EQ(position_confidence::A_2M, position_confidence_from_int(8)); + EXPECT_EQ(position_confidence::A_1M, position_confidence_from_int(9)); + EXPECT_EQ(position_confidence::A_50CM, position_confidence_from_int(10)); + EXPECT_EQ(position_confidence::A_20CM, position_confidence_from_int(11)); + EXPECT_EQ(position_confidence::A_10CM, position_confidence_from_int(12)); + EXPECT_EQ(position_confidence::A_5CM, position_confidence_from_int(13)); + EXPECT_EQ(position_confidence::A_2CM, position_confidence_from_int(14)); + EXPECT_EQ(position_confidence::A_1CM, position_confidence_from_int(15)); + EXPECT_THROW( position_confidence_from_int(16), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/size_value_confidence_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/size_value_confidence_test.cpp new file mode 100644 index 000000000..d10b8eae5 --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/size_value_confidence_test.cpp @@ -0,0 +1,36 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(size_value_confidence_test, test_from_int){ + EXPECT_EQ(size_value_confidence::UNAVAILABLE, size_value_confidence_from_int(0)); + EXPECT_EQ(size_value_confidence::SIZE_100, size_value_confidence_from_int(1)); + EXPECT_EQ(size_value_confidence::SIZE_50, size_value_confidence_from_int(2)); + EXPECT_EQ(size_value_confidence::SIZE_20, size_value_confidence_from_int(3)); + EXPECT_EQ(size_value_confidence::SIZE_10, size_value_confidence_from_int(4)); + EXPECT_EQ(size_value_confidence::SIZE_5, size_value_confidence_from_int(5)); + EXPECT_EQ(size_value_confidence::SIZE_2, size_value_confidence_from_int(6)); + EXPECT_EQ(size_value_confidence::SIZE_1, size_value_confidence_from_int(7)); + EXPECT_EQ(size_value_confidence::SIZE_0_5, size_value_confidence_from_int(8)); + EXPECT_EQ(size_value_confidence::SIZE_0_2, size_value_confidence_from_int(9)); + EXPECT_EQ(size_value_confidence::SIZE_0_1, size_value_confidence_from_int(10)); + EXPECT_EQ(size_value_confidence::SIZE_0_05, size_value_confidence_from_int(11)); + EXPECT_EQ(size_value_confidence::SIZE_0_02, size_value_confidence_from_int(12)); + EXPECT_EQ(size_value_confidence::SIZE_0_01, size_value_confidence_from_int(13)); + EXPECT_THROW( size_value_confidence_from_int(14), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/speed_confidence_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/speed_confidence_test.cpp new file mode 100644 index 000000000..113f80246 --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/speed_confidence_test.cpp @@ -0,0 +1,30 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(speed_confidence, test_from_int){ + EXPECT_EQ(speed_confidence::UNAVAILABLE, speed_confidence_from_int(0)); + EXPECT_EQ(speed_confidence::PREC_100ms, speed_confidence_from_int(1)); + EXPECT_EQ(speed_confidence::PREC_10ms, speed_confidence_from_int(2)); + EXPECT_EQ(speed_confidence::PREC_5ms, speed_confidence_from_int(3)); + EXPECT_EQ(speed_confidence::PREC_1ms, speed_confidence_from_int(4)); + EXPECT_EQ(speed_confidence::PREC_0_1ms, speed_confidence_from_int(5)); + EXPECT_EQ(speed_confidence::PREC_0_05ms, speed_confidence_from_int(6)); + EXPECT_EQ(speed_confidence::PREC_0_01ms, speed_confidence_from_int(7)); + EXPECT_THROW( speed_confidence_from_int(8), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/sensor_data_sharing_msg/time_confidence_test.cpp b/streets_utils/streets_messages/test/sensor_data_sharing_msg/time_confidence_test.cpp new file mode 100644 index 000000000..c6bec601d --- /dev/null +++ b/streets_utils/streets_messages/test/sensor_data_sharing_msg/time_confidence_test.cpp @@ -0,0 +1,62 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +using namespace streets_utils::messages::sdsm; + +TEST(time_confidence_test, test_from_int){ + EXPECT_EQ(time_confidence::UNAVAILABLE, time_confidence_from_int(0)); + EXPECT_EQ(time_confidence::TIME_100_000, time_confidence_from_int(1)); + EXPECT_EQ(time_confidence::TIME_050_000, time_confidence_from_int(2)); + EXPECT_EQ(time_confidence::TIME_020_000, time_confidence_from_int(3)); + EXPECT_EQ(time_confidence::TIME_010_000, time_confidence_from_int(4)); + EXPECT_EQ(time_confidence::TIME_002_000, time_confidence_from_int(5)); + EXPECT_EQ(time_confidence::TIME_001_000, time_confidence_from_int(6)); + EXPECT_EQ(time_confidence::TIME_000_500, time_confidence_from_int(7)); + EXPECT_EQ(time_confidence::TIME_000_200, time_confidence_from_int(8)); + EXPECT_EQ(time_confidence::TIME_000_100, time_confidence_from_int(9)); + EXPECT_EQ(time_confidence::TIME_000_050, time_confidence_from_int(10)); + EXPECT_EQ(time_confidence::TIME_000_020, time_confidence_from_int(11)); + EXPECT_EQ(time_confidence::TIME_000_010, time_confidence_from_int(12)); + EXPECT_EQ(time_confidence::TIME_000_005, time_confidence_from_int(13)); + EXPECT_EQ(time_confidence::TIME_000_002, time_confidence_from_int(14)); + EXPECT_EQ(time_confidence::TIME_000_001, time_confidence_from_int(15)); + EXPECT_EQ(time_confidence::TIME_000_000_5, time_confidence_from_int(16)); + EXPECT_EQ(time_confidence::TIME_000_000_2, time_confidence_from_int(17)); + EXPECT_EQ(time_confidence::TIME_000_000_1, time_confidence_from_int(18)); + EXPECT_EQ(time_confidence::TIME_000_000_05, time_confidence_from_int(19)); + EXPECT_EQ(time_confidence::TIME_000_000_02, time_confidence_from_int(20)); + EXPECT_EQ(time_confidence::TIME_000_000_01, time_confidence_from_int(21)); + EXPECT_EQ(time_confidence::TIME_000_000_005, time_confidence_from_int(22)); + EXPECT_EQ(time_confidence::TIME_000_000_002, time_confidence_from_int(23)); + EXPECT_EQ(time_confidence::TIME_000_000_001, time_confidence_from_int(24)); + EXPECT_EQ(time_confidence::TIME_000_000_000_5, time_confidence_from_int(25)); + EXPECT_EQ(time_confidence::TIME_000_000_000_2, time_confidence_from_int(26)); + EXPECT_EQ(time_confidence::TIME_000_000_000_1, time_confidence_from_int(27)); + EXPECT_EQ(time_confidence::TIME_000_000_000_05, time_confidence_from_int(28)); + EXPECT_EQ(time_confidence::TIME_000_000_000_02, time_confidence_from_int(29)); + EXPECT_EQ(time_confidence::TIME_000_000_000_01, time_confidence_from_int(30)); + EXPECT_EQ(time_confidence::TIME_000_000_000_005, time_confidence_from_int(31)); + EXPECT_EQ(time_confidence::TIME_000_000_000_002, time_confidence_from_int(32)); + EXPECT_EQ(time_confidence::TIME_000_000_000_001, time_confidence_from_int(33)); + EXPECT_EQ(time_confidence::TIME_000_000_000_000_5, time_confidence_from_int(34)); + EXPECT_EQ(time_confidence::TIME_000_000_000_000_2, time_confidence_from_int(35)); + EXPECT_EQ(time_confidence::TIME_000_000_000_000_1, time_confidence_from_int(36)); + EXPECT_EQ(time_confidence::TIME_000_000_000_000_05, time_confidence_from_int(37)); + EXPECT_EQ(time_confidence::TIME_000_000_000_000_02, time_confidence_from_int(38)); + EXPECT_EQ(time_confidence::TIME_000_000_000_000_01, time_confidence_from_int(39)); + EXPECT_THROW( time_confidence_from_int(40), std::invalid_argument); + +} \ No newline at end of file diff --git a/streets_utils/streets_messages/test/serializers/sensor_data_sharing_msg_json_serialization_test.cpp b/streets_utils/streets_messages/test/serializers/sensor_data_sharing_msg_json_serialization_test.cpp new file mode 100644 index 000000000..af368e19f --- /dev/null +++ b/streets_utils/streets_messages/test/serializers/sensor_data_sharing_msg_json_serialization_test.cpp @@ -0,0 +1,976 @@ +// Copyright 2019-2023 Leidos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include + +using namespace streets_utils::messages::sdsm; +using namespace streets_utils::json_utils; + +TEST(sensor_data_sharing_msg_json_serialization_test, confirm_required_properties_max_value) { + sensor_data_sharing_msg msg; + msg._equipment_type = equipment_type::RSU; + msg._msg_count = 255; + msg._source_id = "00000001"; + msg._ref_positon._latitude=900000001; + msg._ref_positon._longitude=1800000001; + msg._time_stamp.second = 65535; + msg._time_stamp.minute = 60; + msg._time_stamp.hour= 31; + msg._time_stamp.day = 31; + msg._time_stamp.month = 12; + msg._time_stamp.year = 4095; // Max + msg._ref_position_confidence._semi_major_axis_accuracy = 255; + msg._ref_position_confidence._semi_minor_axis_accuracy = 255; + msg._ref_position_confidence._semi_major_axis_orientation = 65535; + // Add Detected Object + detected_object_data detected_object; + detected_object._detected_object_common_data._object_id = 65525; + detected_object._detected_object_common_data._object_type = object_type::ANIMAL; + detected_object._detected_object_common_data._classification_confidence = 101; + detected_object._detected_object_common_data._heading = 28800; + detected_object._detected_object_common_data._heading_confidence = heading_confidence::PREC_01_deg; + detected_object._detected_object_common_data._position_offset._offset_x = 32767; + detected_object._detected_object_common_data._position_offset._offset_y = 32767; + detected_object._detected_object_common_data._position_offset._offset_z = 32767; + detected_object._detected_object_common_data._pos_confidence._position_confidence = position_confidence::A_10CM; + detected_object._detected_object_common_data._pos_confidence._elevation_confidence = position_confidence::A_1M; + detected_object._detected_object_common_data._speed = 8191; + detected_object._detected_object_common_data._time_measurement_offset = 1500; + detected_object._detected_object_common_data._time_confidence = time_confidence::TIME_000_000_002; + msg._objects.push_back(detected_object); + // Serialize Msg + std::string json = to_json(msg); + // String is not empty + EXPECT_FALSE(json.empty()); + // Confirm result has msg information in desired structure + auto result = parse_json(json); + + // Confirm timestamp data, Should fail at assert statements since if this require property is + // not available the other calls will fail + ASSERT_TRUE(result.HasMember("sdsm_time_stamp")); + ASSERT_TRUE(result.FindMember("sdsm_time_stamp")->value.IsObject()); + EXPECT_EQ( msg._time_stamp.second, result["sdsm_time_stamp"]["second"].GetInt()); + EXPECT_EQ( msg._time_stamp.minute, result["sdsm_time_stamp"]["minute"].GetInt()); + EXPECT_EQ( msg._time_stamp.hour, result["sdsm_time_stamp"]["hour"].GetInt()); + EXPECT_EQ( msg._time_stamp.day, result["sdsm_time_stamp"]["day"].GetInt()); + EXPECT_EQ( msg._time_stamp.month, result["sdsm_time_stamp"]["month"].GetInt()); + EXPECT_EQ( msg._time_stamp.year, result["sdsm_time_stamp"]["year"].GetInt()); + + EXPECT_EQ( msg._msg_count, result["msg_cnt"].GetUint()); + EXPECT_EQ( msg._source_id, result["source_id"].GetString()); + EXPECT_EQ( static_cast(msg._equipment_type), result["equipment_type"].GetInt()); + // Confirm ref position confidence + ASSERT_TRUE(result.HasMember("ref_pos_xy_conf")); + ASSERT_TRUE(result.FindMember("ref_pos_xy_conf")->value.IsObject()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_accuracy, result["ref_pos_xy_conf"]["semi_major"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_minor_axis_accuracy, result["ref_pos_xy_conf"]["semi_minor"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_orientation, result["ref_pos_xy_conf"]["orientation"].GetInt()); + // Confirm optional elevation parameter is not present + EXPECT_FALSE( result.HasMember("ref_pos_el_conf")); + // Confirm ref position + ASSERT_TRUE(result.HasMember("ref_pos")); + ASSERT_TRUE(result.FindMember("ref_pos")->value.IsObject()); + EXPECT_EQ( msg._ref_positon._longitude, result["ref_pos"]["long"].GetInt64()); + EXPECT_EQ( msg._ref_positon._latitude, result["ref_pos"]["lat"].GetInt64()); + // Optional parameter is not present + EXPECT_FALSE(result["ref_pos"].HasMember("elevation")); + + // Confirm List of detected object exists + ASSERT_TRUE(result.HasMember("objects")); + ASSERT_TRUE(result.FindMember("objects")->value.IsArray()); + ASSERT_EQ(1,result.FindMember("objects")->value.GetArray().Size()); + ASSERT_TRUE(result["objects"].GetArray()[0].IsObject()); + // Confirm object properties + auto object = result["objects"].GetArray()[0].GetObject()["detected_object_data"].GetObject(); + // Assert Object has common data + ASSERT_TRUE(object.HasMember("detected_object_common_data")); + + ASSERT_TRUE(object.FindMember("detected_object_common_data")->value.IsObject()); + ASSERT_FALSE(object.HasMember("detected_object_optional_data")); + // Retreive Object common data + auto object_common_data = object["detected_object_common_data"].GetObject(); + // Retreive Object common data from + auto msg_object_common_data = msg._objects[0]._detected_object_common_data; + // Confirm Object common data properties + EXPECT_EQ(static_cast(msg_object_common_data._object_type), object_common_data["obj_type"].GetUint()); + EXPECT_EQ(msg_object_common_data._classification_confidence, object_common_data["obj_type_cfd"].GetUint()); + EXPECT_EQ(msg_object_common_data._object_id, object_common_data["object_id"].GetUint()); + EXPECT_EQ(msg_object_common_data._time_measurement_offset, object_common_data["measurement_time"].GetInt()); + EXPECT_EQ(static_cast(msg_object_common_data._time_confidence), object_common_data["time_confidence"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed, object_common_data["speed"].GetUint()); + EXPECT_EQ(msg_object_common_data._heading, object_common_data["heading"].GetUint()); + EXPECT_EQ(static_cast(msg_object_common_data._heading_confidence), object_common_data["heading_conf"].GetUint()); + // Test Optional properties not present + EXPECT_FALSE(object_common_data.HasMember("speed_z")); + EXPECT_FALSE(object_common_data.HasMember("speed_confidence_z")); + EXPECT_FALSE(object_common_data.HasMember("accel_4_way")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_x")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_y")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_yaw")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_z")); + +} + +TEST(sensor_data_sharing_msg_json_serialization_test, confirm_required_properties_min_value) { + sensor_data_sharing_msg msg; + msg._equipment_type = equipment_type::OBU; + msg._msg_count = 1; + msg._source_id = "00000001"; + msg._ref_positon._latitude=-90000000; + msg._ref_positon._longitude=-1800000000; + msg._time_stamp.second = 0; + msg._time_stamp.minute = 0; + msg._time_stamp.hour= 0; + msg._time_stamp.day = 0; + msg._time_stamp.month = 0; + msg._time_stamp.year = 0; + msg._ref_position_confidence._semi_major_axis_accuracy = 0; + msg._ref_position_confidence._semi_minor_axis_accuracy = 0; + msg._ref_position_confidence._semi_major_axis_orientation = 0; + // Add Detected Object + detected_object_data detected_object; + detected_object._detected_object_common_data._object_id = 0; + detected_object._detected_object_common_data._object_type = object_type::VEHICLE; + detected_object._detected_object_common_data._classification_confidence = 0; + detected_object._detected_object_common_data._heading = 0; + detected_object._detected_object_common_data._heading_confidence = heading_confidence::PREC_0_0125_deg; + detected_object._detected_object_common_data._position_offset._offset_x = -32767; + detected_object._detected_object_common_data._position_offset._offset_y = -32767; + detected_object._detected_object_common_data._position_offset._offset_z = -32767; + detected_object._detected_object_common_data._pos_confidence._position_confidence = position_confidence::A_500M; + detected_object._detected_object_common_data._pos_confidence._elevation_confidence = position_confidence::A_1CM; + detected_object._detected_object_common_data._speed = 0; + detected_object._detected_object_common_data._time_measurement_offset = -1500; + detected_object._detected_object_common_data._time_confidence = time_confidence::TIME_000_000_000_000_01; + msg._objects.push_back(detected_object); + // Serialize Msg + std::string json = to_json(msg); + // String is not empty + EXPECT_FALSE(json.empty()); + // Confirm result has msg information in desired structure + auto result = parse_json(json); + + // Confirm timestamp data, Should fail at assert statements since if this require property is + // not available the other calls will fail + ASSERT_TRUE(result.HasMember("sdsm_time_stamp")); + ASSERT_TRUE(result.FindMember("sdsm_time_stamp")->value.IsObject()); + EXPECT_EQ( msg._time_stamp.second, result["sdsm_time_stamp"]["second"].GetInt()); + EXPECT_EQ( msg._time_stamp.minute, result["sdsm_time_stamp"]["minute"].GetInt()); + EXPECT_EQ( msg._time_stamp.hour, result["sdsm_time_stamp"]["hour"].GetInt()); + EXPECT_EQ( msg._time_stamp.day, result["sdsm_time_stamp"]["day"].GetInt()); + EXPECT_EQ( msg._time_stamp.month, result["sdsm_time_stamp"]["month"].GetInt()); + EXPECT_EQ( msg._time_stamp.year, result["sdsm_time_stamp"]["year"].GetInt()); + + EXPECT_EQ( msg._msg_count, result["msg_cnt"].GetUint()); + EXPECT_EQ( msg._source_id, result["source_id"].GetString()); + EXPECT_EQ( static_cast(msg._equipment_type), result["equipment_type"].GetInt()); + // Confirm ref position confidence + ASSERT_TRUE(result.HasMember("ref_pos_xy_conf")); + ASSERT_TRUE(result.FindMember("ref_pos_xy_conf")->value.IsObject()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_accuracy, result["ref_pos_xy_conf"]["semi_major"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_minor_axis_accuracy, result["ref_pos_xy_conf"]["semi_minor"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_orientation, result["ref_pos_xy_conf"]["orientation"].GetInt()); + // Confirm optional elevation parameter is not present + EXPECT_FALSE( result.HasMember("ref_pos_el_conf")); + // Confirm ref position + ASSERT_TRUE(result.HasMember("ref_pos")); + ASSERT_TRUE(result.FindMember("ref_pos")->value.IsObject()); + EXPECT_EQ( msg._ref_positon._longitude, result["ref_pos"]["long"].GetInt64()); + EXPECT_EQ( msg._ref_positon._latitude, result["ref_pos"]["lat"].GetInt64()); + // Optional parameter is not present + EXPECT_FALSE(result["ref_pos"].HasMember("elevation")); + + // Confirm List of detected object exists + ASSERT_TRUE(result.HasMember("objects")); + ASSERT_TRUE(result.FindMember("objects")->value.IsArray()); + ASSERT_EQ(1,result.FindMember("objects")->value.GetArray().Size()); + ASSERT_TRUE(result["objects"].GetArray()[0].IsObject()); + // Confirm object properties + auto object = result["objects"].GetArray()[0].GetObject()["detected_object_data"].GetObject(); + // Assert Object has common data + ASSERT_TRUE(object.HasMember("detected_object_common_data")); + ASSERT_TRUE(object.FindMember("detected_object_common_data")->value.IsObject()); + ASSERT_FALSE(object.HasMember("detected_object_optional_data")); + // Retreive Object common data + auto object_common_data = object["detected_object_common_data"].GetObject(); + // Retreive Object common data from + auto msg_object_common_data = msg._objects[0]._detected_object_common_data; + // Confirm Object common data properties + EXPECT_EQ(static_cast(msg_object_common_data._object_type), object_common_data["obj_type"].GetUint()); + EXPECT_EQ(msg_object_common_data._classification_confidence, object_common_data["obj_type_cfd"].GetUint()); + EXPECT_EQ(msg_object_common_data._object_id, object_common_data["object_id"].GetUint()); + EXPECT_EQ(msg_object_common_data._time_measurement_offset, object_common_data["measurement_time"].GetInt()); + EXPECT_EQ(static_cast(msg_object_common_data._time_confidence), object_common_data["time_confidence"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed, object_common_data["speed"].GetUint()); + EXPECT_EQ(msg_object_common_data._heading, object_common_data["heading"].GetUint()); + EXPECT_EQ(static_cast(msg_object_common_data._heading_confidence), object_common_data["heading_conf"].GetUint()); + // Test Optional properties not present + EXPECT_FALSE(object_common_data.HasMember("speed_z")); + EXPECT_FALSE(object_common_data.HasMember("speed_confidence_z")); + EXPECT_FALSE(object_common_data.HasMember("accel_4_way")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_x")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_y")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_yaw")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_z")); + +} + +TEST(sensor_data_sharing_msg_json_serialization_test, confirm_optional_vru_human_properties) { + sensor_data_sharing_msg msg; + msg._equipment_type = equipment_type::OBU; + msg._msg_count = 1; + msg._source_id = "00000001"; + msg._ref_positon._latitude=-90000000; + msg._ref_positon._longitude=-1800000000; + msg._ref_positon._elevation = -32768; + msg._time_stamp.second = 0; + msg._time_stamp.minute = 0; + msg._time_stamp.hour= 0; + msg._time_stamp.day = 0; + msg._time_stamp.month = 0; + msg._time_stamp.year = 0; + msg._ref_position_confidence._semi_major_axis_accuracy = 0; + msg._ref_position_confidence._semi_minor_axis_accuracy = 0; + msg._ref_position_confidence._semi_major_axis_orientation = 0; + msg._ref_position_elevation_confidence = position_confidence::A_10M; + // Add Detected Object + detected_object_data detected_object; + detected_object._detected_object_common_data._object_id = 0; + detected_object._detected_object_common_data._object_type = object_type::VEHICLE; + detected_object._detected_object_common_data._classification_confidence = 0; + detected_object._detected_object_common_data._heading = 0; + detected_object._detected_object_common_data._heading_confidence = heading_confidence::PREC_0_0125_deg; + detected_object._detected_object_common_data._position_offset._offset_x = -32767; + detected_object._detected_object_common_data._position_offset._offset_y = -32767; + detected_object._detected_object_common_data._position_offset._offset_z = -32767; + detected_object._detected_object_common_data._pos_confidence._position_confidence = position_confidence::A_500M; + detected_object._detected_object_common_data._pos_confidence._elevation_confidence = position_confidence::A_1CM; + detected_object._detected_object_common_data._speed = 0; + detected_object._detected_object_common_data._speed_z = 8191; + detected_object._detected_object_common_data._speed_confidence = speed_confidence::UNAVAILABLE; + detected_object._detected_object_common_data._speed_z_confidence = speed_confidence::PREC_100ms; + // Create acceleration 4 way + acceleration_set_4_way accel_set; + accel_set._lateral_accel = 2001; + accel_set._longitudinal_accel =2001; + accel_set._vertical_accel = 127; + accel_set._yaw_rate = 32767; + detected_object._detected_object_common_data._acceleration_4_way= accel_set; + // Create acceleration confidence set + detected_object._detected_object_common_data._lateral_acceleration_confidence = acceleration_confidence::ACCL_0_1; + detected_object._detected_object_common_data._longitudinal_acceleration_confidence = acceleration_confidence::ACCL_100; + detected_object._detected_object_common_data._vertical_accelaration_confidence = acceleration_confidence::ACCL_0_01; + detected_object._detected_object_common_data._yaw_rate_confidence = angular_velocity_confidence::UNAVAILABLE; + + detected_object._detected_object_common_data._time_measurement_offset = -1500; + detected_object._detected_object_common_data._time_confidence = time_confidence::TIME_000_000_000_000_01; + // Add Detected VRU Optional data + detected_vru_data detected_vru; + detected_vru._attachment = attachment::WHEEL_CHAIR; + detected_vru._personal_device_user_type = personal_device_user_type::PEDESTRIAN; + detected_vru._attachment_radius = 200; + detected_vru._propulsion = human_propelled_type::WHEELCHAIR; + ASSERT_TRUE( detected_vru._propulsion.has_value()); + detected_object._detected_object_optional_data = detected_vru; + msg._objects.push_back(detected_object); + // Serialize Msg + std::string json = to_json(msg); + // String is not empty + EXPECT_FALSE(json.empty()); + // Confirm result has msg information in desired structure + auto result = parse_json(json); + + // Confirm timestamp data, Should fail at assert statements since if this require property is + // not available the other calls will fail + ASSERT_TRUE(result.HasMember("sdsm_time_stamp")); + ASSERT_TRUE(result.FindMember("sdsm_time_stamp")->value.IsObject()); + EXPECT_EQ( msg._time_stamp.second, result["sdsm_time_stamp"]["second"].GetInt()); + EXPECT_EQ( msg._time_stamp.minute, result["sdsm_time_stamp"]["minute"].GetInt()); + EXPECT_EQ( msg._time_stamp.hour, result["sdsm_time_stamp"]["hour"].GetInt()); + EXPECT_EQ( msg._time_stamp.day, result["sdsm_time_stamp"]["day"].GetInt()); + EXPECT_EQ( msg._time_stamp.month, result["sdsm_time_stamp"]["month"].GetInt()); + EXPECT_EQ( msg._time_stamp.year, result["sdsm_time_stamp"]["year"].GetInt()); + + EXPECT_EQ( msg._msg_count, result["msg_cnt"].GetUint()); + EXPECT_EQ( msg._source_id, result["source_id"].GetString()); + EXPECT_EQ( static_cast(msg._equipment_type), result["equipment_type"].GetInt()); + // Confirm ref position confidence + ASSERT_TRUE(result.HasMember("ref_pos_xy_conf")); + ASSERT_TRUE(result.FindMember("ref_pos_xy_conf")->value.IsObject()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_accuracy, result["ref_pos_xy_conf"]["semi_major"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_minor_axis_accuracy, result["ref_pos_xy_conf"]["semi_minor"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_orientation, result["ref_pos_xy_conf"]["orientation"].GetInt()); + // Confirm optional elevation parameter is present + EXPECT_TRUE( result.HasMember("ref_pos_el_conf")); + // Confirm ref position + ASSERT_TRUE(result.HasMember("ref_pos")); + ASSERT_TRUE(result.FindMember("ref_pos")->value.IsObject()); + EXPECT_EQ( msg._ref_positon._longitude, result["ref_pos"]["long"].GetInt64()); + EXPECT_EQ( msg._ref_positon._latitude, result["ref_pos"]["lat"].GetInt64()); + // Confirm optional elevation parameter is present + EXPECT_EQ( msg._ref_positon._elevation, result["ref_pos"]["elevation"].GetInt()); + EXPECT_EQ( msg._ref_position_elevation_confidence, position_confidence_from_int( result["ref_pos_el_conf"].GetUint())); + + // Confirm List of detected object exists + ASSERT_TRUE(result.HasMember("objects")); + ASSERT_TRUE(result.FindMember("objects")->value.IsArray()); + ASSERT_EQ(1,result.FindMember("objects")->value.GetArray().Size()); + ASSERT_TRUE(result["objects"].GetArray()[0].IsObject()); + // Confirm object properties + auto object = result["objects"].GetArray()[0].GetObject()["detected_object_data"].GetObject(); + // Assert Object has common data + ASSERT_TRUE(object.HasMember("detected_object_common_data")); + ASSERT_TRUE(object.FindMember("detected_object_common_data")->value.IsObject()); + // Retreive Object common data + auto object_common_data = object["detected_object_common_data"].GetObject(); + // Retreive Object common data from + auto msg_object_common_data = msg._objects[0]._detected_object_common_data; + // Confirm Object common data properties + EXPECT_EQ(static_cast(msg_object_common_data._object_type), object_common_data["obj_type"].GetUint()); + EXPECT_EQ(msg_object_common_data._classification_confidence, object_common_data["obj_type_cfd"].GetUint()); + EXPECT_EQ(msg_object_common_data._object_id, object_common_data["object_id"].GetUint()); + EXPECT_EQ(msg_object_common_data._time_measurement_offset, object_common_data["measurement_time"].GetInt()); + EXPECT_EQ(static_cast(msg_object_common_data._time_confidence), object_common_data["time_confidence"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed, object_common_data["speed"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed_confidence, speed_confidence_from_int (object_common_data["speed_confidence"].GetUint())); + EXPECT_EQ(msg_object_common_data._heading, object_common_data["heading"].GetUint()); + EXPECT_EQ(static_cast(msg_object_common_data._heading_confidence), object_common_data["heading_conf"].GetUint()); + // Test Optional properties present + EXPECT_EQ(msg_object_common_data._speed_z, object_common_data["speed_z"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed_z_confidence, speed_confidence_from_int(object_common_data["speed_confidence_z"].GetUint()) ); + // Confirm accel 4 way values + EXPECT_TRUE(object_common_data.HasMember("accel_4_way")); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_lateral_accel, object_common_data["accel_4_way"]["lat"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_longitudinal_accel, object_common_data["accel_4_way"]["long"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_vertical_accel, object_common_data["accel_4_way"]["vert"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_yaw_rate, object_common_data["accel_4_way"]["yaw"].GetInt()); + // Confirm acceleration confidence + EXPECT_EQ(msg_object_common_data._lateral_acceleration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_x"].GetUint())); + EXPECT_EQ(msg_object_common_data._longitudinal_acceleration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_y"].GetUint())); + EXPECT_EQ(msg_object_common_data._vertical_accelaration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_z"].GetUint())); + EXPECT_EQ(msg_object_common_data._yaw_rate_confidence,angular_velocity_confidence_from_int( object_common_data["acc_cfd_yaw"].GetUint())); + // Expect vru optional fields information + ASSERT_TRUE(object.HasMember("detected_object_optional_data")); + ASSERT_TRUE(object["detected_object_optional_data"].HasMember("detected_vru_data") ); + auto msg_object_optional_data = std::get(msg._objects[0]._detected_object_optional_data.value()); + auto option_vru_json_data = object["detected_object_optional_data"]["detected_vru_data"].GetObject(); + EXPECT_EQ( msg_object_optional_data._attachment.value(), attachment_from_int( option_vru_json_data["attachment"].GetUint())); + EXPECT_EQ( msg_object_optional_data._attachment_radius.value(), option_vru_json_data["radius"].GetUint()); + EXPECT_EQ( msg_object_optional_data._personal_device_user_type.value(), personal_device_user_type_from_int(option_vru_json_data["basic_type"].GetUint())); + EXPECT_EQ( std::get(msg_object_optional_data._propulsion.value()), human_propelled_type_from_int(option_vru_json_data["propulsion"]["human"].GetUint())); +} +TEST(sensor_data_sharing_msg_json_serialization_test, confirm_optional_vru_animal_properties) { + sensor_data_sharing_msg msg; + msg._equipment_type = equipment_type::OBU; + msg._msg_count = 1; + msg._source_id = "00000001"; + msg._ref_positon._latitude=-90000000; + msg._ref_positon._longitude=-1800000000; + msg._ref_positon._elevation = -32768; + msg._time_stamp.second = 0; + msg._time_stamp.minute = 0; + msg._time_stamp.hour= 0; + msg._time_stamp.day = 0; + msg._time_stamp.month = 0; + msg._time_stamp.year = 0; + msg._ref_position_confidence._semi_major_axis_accuracy = 0; + msg._ref_position_confidence._semi_minor_axis_accuracy = 0; + msg._ref_position_confidence._semi_major_axis_orientation = 0; + msg._ref_position_elevation_confidence = position_confidence::A_10M; + // Add Detected Object + detected_object_data detected_object; + detected_object._detected_object_common_data._object_id = 0; + detected_object._detected_object_common_data._object_type = object_type::VEHICLE; + detected_object._detected_object_common_data._classification_confidence = 0; + detected_object._detected_object_common_data._heading = 0; + detected_object._detected_object_common_data._heading_confidence = heading_confidence::PREC_0_0125_deg; + detected_object._detected_object_common_data._position_offset._offset_x = -32767; + detected_object._detected_object_common_data._position_offset._offset_y = -32767; + detected_object._detected_object_common_data._position_offset._offset_z = -32767; + detected_object._detected_object_common_data._pos_confidence._position_confidence = position_confidence::A_500M; + detected_object._detected_object_common_data._pos_confidence._elevation_confidence = position_confidence::A_1CM; + detected_object._detected_object_common_data._speed = 0; + detected_object._detected_object_common_data._speed_z = 8191; + detected_object._detected_object_common_data._speed_confidence = speed_confidence::UNAVAILABLE; + detected_object._detected_object_common_data._speed_z_confidence = speed_confidence::PREC_100ms; + // Create acceleration 4 way + acceleration_set_4_way accel_set; + accel_set._lateral_accel = 2001; + accel_set._longitudinal_accel =2001; + accel_set._vertical_accel = 127; + accel_set._yaw_rate = 32767; + detected_object._detected_object_common_data._acceleration_4_way= accel_set; + // Create acceleration confidence set + detected_object._detected_object_common_data._lateral_acceleration_confidence = acceleration_confidence::ACCL_0_1; + detected_object._detected_object_common_data._longitudinal_acceleration_confidence = acceleration_confidence::ACCL_100; + detected_object._detected_object_common_data._vertical_accelaration_confidence = acceleration_confidence::ACCL_0_01; + detected_object._detected_object_common_data._yaw_rate_confidence = angular_velocity_confidence::UNAVAILABLE; + + + detected_object._detected_object_common_data._time_measurement_offset = -1500; + detected_object._detected_object_common_data._time_confidence = time_confidence::TIME_000_000_000_000_01; + // Add Detected VRU Optional data + detected_vru_data detected_vru; + detected_vru._attachment = attachment::WHEEL_CHAIR; + detected_vru._personal_device_user_type = personal_device_user_type::PEDESTRIAN; + detected_vru._attachment_radius = 200; + detected_vru._propulsion = animal_propelled_type::ANIMAL_DRAWN_CARRIAGE; + ASSERT_TRUE( detected_vru._propulsion.has_value()); + detected_object._detected_object_optional_data = detected_vru; + msg._objects.push_back(detected_object); + // Serialize Msg + std::string json = to_json(msg); + // String is not empty + EXPECT_FALSE(json.empty()); + // Confirm result has msg information in desired structure + auto result = parse_json(json); + + // Confirm timestamp data, Should fail at assert statements since if this require property is + // not available the other calls will fail + ASSERT_TRUE(result.HasMember("sdsm_time_stamp")); + ASSERT_TRUE(result.FindMember("sdsm_time_stamp")->value.IsObject()); + EXPECT_EQ( msg._time_stamp.second, result["sdsm_time_stamp"]["second"].GetInt()); + EXPECT_EQ( msg._time_stamp.minute, result["sdsm_time_stamp"]["minute"].GetInt()); + EXPECT_EQ( msg._time_stamp.hour, result["sdsm_time_stamp"]["hour"].GetInt()); + EXPECT_EQ( msg._time_stamp.day, result["sdsm_time_stamp"]["day"].GetInt()); + EXPECT_EQ( msg._time_stamp.month, result["sdsm_time_stamp"]["month"].GetInt()); + EXPECT_EQ( msg._time_stamp.year, result["sdsm_time_stamp"]["year"].GetInt()); + + EXPECT_EQ( msg._msg_count, result["msg_cnt"].GetUint()); + EXPECT_EQ( msg._source_id, result["source_id"].GetString()); + EXPECT_EQ( static_cast(msg._equipment_type), result["equipment_type"].GetInt()); + // Confirm ref position confidence + ASSERT_TRUE(result.HasMember("ref_pos_xy_conf")); + ASSERT_TRUE(result.FindMember("ref_pos_xy_conf")->value.IsObject()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_accuracy, result["ref_pos_xy_conf"]["semi_major"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_minor_axis_accuracy, result["ref_pos_xy_conf"]["semi_minor"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_orientation, result["ref_pos_xy_conf"]["orientation"].GetInt()); + // Confirm optional elevation parameter is present + EXPECT_TRUE( result.HasMember("ref_pos_el_conf")); + // Confirm ref position + ASSERT_TRUE(result.HasMember("ref_pos")); + ASSERT_TRUE(result.FindMember("ref_pos")->value.IsObject()); + EXPECT_EQ( msg._ref_positon._longitude, result["ref_pos"]["long"].GetInt64()); + EXPECT_EQ( msg._ref_positon._latitude, result["ref_pos"]["lat"].GetInt64()); + // Confirm optional elevation parameter is present + EXPECT_EQ( msg._ref_positon._elevation, result["ref_pos"]["elevation"].GetInt()); + EXPECT_EQ( msg._ref_position_elevation_confidence, position_confidence_from_int( result["ref_pos_el_conf"].GetUint())); + + // Confirm List of detected object exists + ASSERT_TRUE(result.HasMember("objects")); + ASSERT_TRUE(result.FindMember("objects")->value.IsArray()); + ASSERT_EQ(1,result.FindMember("objects")->value.GetArray().Size()); + ASSERT_TRUE(result["objects"].GetArray()[0].IsObject()); + // Confirm object properties + auto object = result["objects"].GetArray()[0].GetObject()["detected_object_data"].GetObject(); + // Assert Object has common data + ASSERT_TRUE(object.HasMember("detected_object_common_data")); + ASSERT_TRUE(object.FindMember("detected_object_common_data")->value.IsObject()); + // Retreive Object common data + auto object_common_data = object["detected_object_common_data"].GetObject(); + // Retreive Object common data from + auto msg_object_common_data = msg._objects[0]._detected_object_common_data; + // Confirm Object common data properties + EXPECT_EQ(static_cast(msg_object_common_data._object_type), object_common_data["obj_type"].GetUint()); + EXPECT_EQ(msg_object_common_data._classification_confidence, object_common_data["obj_type_cfd"].GetUint()); + EXPECT_EQ(msg_object_common_data._object_id, object_common_data["object_id"].GetUint()); + EXPECT_EQ(msg_object_common_data._time_measurement_offset, object_common_data["measurement_time"].GetInt()); + EXPECT_EQ(static_cast(msg_object_common_data._time_confidence), object_common_data["time_confidence"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed, object_common_data["speed"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed_confidence, speed_confidence_from_int (object_common_data["speed_confidence"].GetUint())); + EXPECT_EQ(msg_object_common_data._heading, object_common_data["heading"].GetUint()); + EXPECT_EQ(static_cast(msg_object_common_data._heading_confidence), object_common_data["heading_conf"].GetUint()); + // Test Optional properties present + EXPECT_EQ(msg_object_common_data._speed_z, object_common_data["speed_z"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed_z_confidence, speed_confidence_from_int(object_common_data["speed_confidence_z"].GetUint()) ); + // Confirm accel 4 way values + EXPECT_TRUE(object_common_data.HasMember("accel_4_way")); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_lateral_accel, object_common_data["accel_4_way"]["lat"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_longitudinal_accel, object_common_data["accel_4_way"]["long"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_vertical_accel, object_common_data["accel_4_way"]["vert"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_yaw_rate, object_common_data["accel_4_way"]["yaw"].GetInt()); + // Confirm acceleration confidence + EXPECT_EQ(msg_object_common_data._lateral_acceleration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_x"].GetUint())); + EXPECT_EQ(msg_object_common_data._longitudinal_acceleration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_y"].GetUint())); + EXPECT_EQ(msg_object_common_data._vertical_accelaration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_z"].GetUint())); + EXPECT_EQ(msg_object_common_data._yaw_rate_confidence,angular_velocity_confidence_from_int( object_common_data["acc_cfd_yaw"].GetUint())); + // Expect vru optional fields information + ASSERT_TRUE(object.HasMember("detected_object_optional_data")); + ASSERT_TRUE(object["detected_object_optional_data"].HasMember("detected_vru_data") ); + auto msg_object_optional_data = std::get(msg._objects[0]._detected_object_optional_data.value()); + auto option_vru_json_data = object["detected_object_optional_data"]["detected_vru_data"].GetObject(); + EXPECT_EQ( msg_object_optional_data._attachment.value(), attachment_from_int( option_vru_json_data["attachment"].GetUint())); + EXPECT_EQ( msg_object_optional_data._attachment_radius.value(), option_vru_json_data["radius"].GetUint()); + EXPECT_EQ( msg_object_optional_data._personal_device_user_type.value(), personal_device_user_type_from_int(option_vru_json_data["basic_type"].GetUint())); + EXPECT_EQ( std::get(msg_object_optional_data._propulsion.value()), animal_propelled_type_from_int(option_vru_json_data["propulsion"]["animal"].GetUint())); +} + + +TEST(sensor_data_sharing_msg_json_serialization_test, confirm_optional_vru_motor_properties) { + sensor_data_sharing_msg msg; + msg._equipment_type = equipment_type::OBU; + msg._msg_count = 1; + msg._source_id = "00000001"; + msg._ref_positon._latitude=-90000000; + msg._ref_positon._longitude=-1800000000; + msg._ref_positon._elevation = -32768; + msg._time_stamp.second = 0; + msg._time_stamp.minute = 0; + msg._time_stamp.hour= 0; + msg._time_stamp.day = 0; + msg._time_stamp.month = 0; + msg._time_stamp.year = 0; + msg._ref_position_confidence._semi_major_axis_accuracy = 0; + msg._ref_position_confidence._semi_minor_axis_accuracy = 0; + msg._ref_position_confidence._semi_major_axis_orientation = 0; + msg._ref_position_elevation_confidence = position_confidence::A_10M; + // Add Detected Object + detected_object_data detected_object; + detected_object._detected_object_common_data._object_id = 0; + detected_object._detected_object_common_data._object_type = object_type::VEHICLE; + detected_object._detected_object_common_data._classification_confidence = 0; + detected_object._detected_object_common_data._heading = 0; + detected_object._detected_object_common_data._heading_confidence = heading_confidence::PREC_0_0125_deg; + detected_object._detected_object_common_data._position_offset._offset_x = -32767; + detected_object._detected_object_common_data._position_offset._offset_y = -32767; + detected_object._detected_object_common_data._position_offset._offset_z = -32767; + detected_object._detected_object_common_data._pos_confidence._position_confidence = position_confidence::A_500M; + detected_object._detected_object_common_data._pos_confidence._elevation_confidence = position_confidence::A_1CM; + detected_object._detected_object_common_data._speed = 0; + detected_object._detected_object_common_data._speed_z = 8191; + detected_object._detected_object_common_data._speed_confidence = speed_confidence::UNAVAILABLE; + detected_object._detected_object_common_data._speed_z_confidence = speed_confidence::PREC_100ms; + // Create acceleration 4 way + acceleration_set_4_way accel_set; + accel_set._lateral_accel = 2001; + accel_set._longitudinal_accel =2001; + accel_set._vertical_accel = 127; + accel_set._yaw_rate = 32767; + detected_object._detected_object_common_data._acceleration_4_way= accel_set; + // Create acceleration confidence set + detected_object._detected_object_common_data._lateral_acceleration_confidence = acceleration_confidence::ACCL_0_1; + detected_object._detected_object_common_data._longitudinal_acceleration_confidence = acceleration_confidence::ACCL_100; + detected_object._detected_object_common_data._vertical_accelaration_confidence = acceleration_confidence::ACCL_0_01; + detected_object._detected_object_common_data._yaw_rate_confidence = angular_velocity_confidence::UNAVAILABLE; + + detected_object._detected_object_common_data._time_measurement_offset = -1500; + detected_object._detected_object_common_data._time_confidence = time_confidence::TIME_000_000_000_000_01; + // Add Detected VRU Optional data + detected_vru_data detected_vru; + detected_vru._attachment = attachment::WHEEL_CHAIR; + detected_vru._personal_device_user_type = personal_device_user_type::PEDESTRIAN; + detected_vru._attachment_radius = 200; + detected_vru._propulsion = motorized_propelled_type::SCOOTER; + ASSERT_TRUE( detected_vru._propulsion.has_value()); + detected_object._detected_object_optional_data = detected_vru; + msg._objects.push_back(detected_object); + // Serialize Msg + std::string json = to_json(msg); + // String is not empty + EXPECT_FALSE(json.empty()); + // Confirm result has msg information in desired structure + auto result = parse_json(json); + + // Confirm timestamp data, Should fail at assert statements since if this require property is + // not available the other calls will fail + ASSERT_TRUE(result.HasMember("sdsm_time_stamp")); + ASSERT_TRUE(result.FindMember("sdsm_time_stamp")->value.IsObject()); + EXPECT_EQ( msg._time_stamp.second, result["sdsm_time_stamp"]["second"].GetInt()); + EXPECT_EQ( msg._time_stamp.minute, result["sdsm_time_stamp"]["minute"].GetInt()); + EXPECT_EQ( msg._time_stamp.hour, result["sdsm_time_stamp"]["hour"].GetInt()); + EXPECT_EQ( msg._time_stamp.day, result["sdsm_time_stamp"]["day"].GetInt()); + EXPECT_EQ( msg._time_stamp.month, result["sdsm_time_stamp"]["month"].GetInt()); + EXPECT_EQ( msg._time_stamp.year, result["sdsm_time_stamp"]["year"].GetInt()); + + EXPECT_EQ( msg._msg_count, result["msg_cnt"].GetUint()); + EXPECT_EQ( msg._source_id, result["source_id"].GetString()); + EXPECT_EQ( static_cast(msg._equipment_type), result["equipment_type"].GetInt()); + // Confirm ref position confidence + ASSERT_TRUE(result.HasMember("ref_pos_xy_conf")); + ASSERT_TRUE(result.FindMember("ref_pos_xy_conf")->value.IsObject()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_accuracy, result["ref_pos_xy_conf"]["semi_major"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_minor_axis_accuracy, result["ref_pos_xy_conf"]["semi_minor"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_orientation, result["ref_pos_xy_conf"]["orientation"].GetInt()); + // Confirm optional elevation parameter is present + EXPECT_TRUE( result.HasMember("ref_pos_el_conf")); + // Confirm ref position + ASSERT_TRUE(result.HasMember("ref_pos")); + ASSERT_TRUE(result.FindMember("ref_pos")->value.IsObject()); + EXPECT_EQ( msg._ref_positon._longitude, result["ref_pos"]["long"].GetInt64()); + EXPECT_EQ( msg._ref_positon._latitude, result["ref_pos"]["lat"].GetInt64()); + // Confirm optional elevation parameter is present + EXPECT_EQ( msg._ref_positon._elevation, result["ref_pos"]["elevation"].GetInt()); + EXPECT_EQ( msg._ref_position_elevation_confidence, position_confidence_from_int( result["ref_pos_el_conf"].GetUint())); + + // Confirm List of detected object exists + ASSERT_TRUE(result.HasMember("objects")); + ASSERT_TRUE(result.FindMember("objects")->value.IsArray()); + ASSERT_EQ(1,result.FindMember("objects")->value.GetArray().Size()); + ASSERT_TRUE(result["objects"].GetArray()[0].IsObject()); + // Confirm object properties + auto object = result["objects"].GetArray()[0].GetObject()["detected_object_data"].GetObject(); + // Assert Object has common data + ASSERT_TRUE(object.HasMember("detected_object_common_data")); + ASSERT_TRUE(object.FindMember("detected_object_common_data")->value.IsObject()); + // Retreive Object common data + auto object_common_data = object["detected_object_common_data"].GetObject(); + // Retreive Object common data from + auto msg_object_common_data = msg._objects[0]._detected_object_common_data; + // Confirm Object common data properties + EXPECT_EQ(static_cast(msg_object_common_data._object_type), object_common_data["obj_type"].GetUint()); + EXPECT_EQ(msg_object_common_data._classification_confidence, object_common_data["obj_type_cfd"].GetUint()); + EXPECT_EQ(msg_object_common_data._object_id, object_common_data["object_id"].GetUint()); + EXPECT_EQ(msg_object_common_data._time_measurement_offset, object_common_data["measurement_time"].GetInt()); + EXPECT_EQ(static_cast(msg_object_common_data._time_confidence), object_common_data["time_confidence"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed, object_common_data["speed"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed_confidence, speed_confidence_from_int (object_common_data["speed_confidence"].GetUint())); + EXPECT_EQ(msg_object_common_data._heading, object_common_data["heading"].GetUint()); + EXPECT_EQ(static_cast(msg_object_common_data._heading_confidence), object_common_data["heading_conf"].GetUint()); + // Test Optional properties present + EXPECT_EQ(msg_object_common_data._speed_z, object_common_data["speed_z"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed_z_confidence, speed_confidence_from_int(object_common_data["speed_confidence_z"].GetUint()) ); + // Confirm accel 4 way values + EXPECT_TRUE(object_common_data.HasMember("accel_4_way")); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_lateral_accel, object_common_data["accel_4_way"]["lat"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_longitudinal_accel, object_common_data["accel_4_way"]["long"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_vertical_accel, object_common_data["accel_4_way"]["vert"].GetInt()); + EXPECT_EQ( msg_object_common_data._acceleration_4_way->_yaw_rate, object_common_data["accel_4_way"]["yaw"].GetInt()); + // Confirm acceleration confidence + EXPECT_EQ(msg_object_common_data._lateral_acceleration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_x"].GetUint())); + EXPECT_EQ(msg_object_common_data._longitudinal_acceleration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_y"].GetUint())); + EXPECT_EQ(msg_object_common_data._vertical_accelaration_confidence,acceleration_confidence_from_int( object_common_data["acc_cfd_z"].GetUint())); + EXPECT_EQ(msg_object_common_data._yaw_rate_confidence,angular_velocity_confidence_from_int( object_common_data["acc_cfd_yaw"].GetUint())); + // Expect vru optional fields information + ASSERT_TRUE(object.HasMember("detected_object_optional_data")); + ASSERT_TRUE(object["detected_object_optional_data"].HasMember("detected_vru_data") ); + auto msg_object_optional_data = std::get(msg._objects[0]._detected_object_optional_data.value()); + auto option_vru_json_data = object["detected_object_optional_data"]["detected_vru_data"].GetObject(); + EXPECT_EQ( msg_object_optional_data._attachment.value(), attachment_from_int( option_vru_json_data["attachment"].GetUint())); + EXPECT_EQ( msg_object_optional_data._attachment_radius.value(), option_vru_json_data["radius"].GetUint()); + EXPECT_EQ( msg_object_optional_data._personal_device_user_type.value(), personal_device_user_type_from_int(option_vru_json_data["basic_type"].GetUint())); + EXPECT_EQ( std::get(msg_object_optional_data._propulsion.value()), motorized_propelled_type_from_int(option_vru_json_data["propulsion"]["motor"].GetUint())); +} + + +TEST(sensor_data_sharing_msg_json_serialization_test, confirm_optional_obstacle_properties) { + sensor_data_sharing_msg msg; + msg._equipment_type = equipment_type::OBU; + msg._msg_count = 1; + msg._source_id = "00000001"; + msg._ref_positon._latitude=-90000000; + msg._ref_positon._longitude=-1800000000; + msg._time_stamp.second = 0; + msg._time_stamp.minute = 0; + msg._time_stamp.hour= 0; + msg._time_stamp.day = 0; + msg._time_stamp.month = 0; + msg._time_stamp.year = 0; + msg._ref_position_confidence._semi_major_axis_accuracy = 0; + msg._ref_position_confidence._semi_minor_axis_accuracy = 0; + msg._ref_position_confidence._semi_major_axis_orientation = 0; + // Add Detected Object + detected_object_data detected_object; + detected_object._detected_object_common_data._object_id = 0; + detected_object._detected_object_common_data._object_type = object_type::VEHICLE; + detected_object._detected_object_common_data._classification_confidence = 0; + detected_object._detected_object_common_data._heading = 0; + detected_object._detected_object_common_data._heading_confidence = heading_confidence::PREC_0_0125_deg; + detected_object._detected_object_common_data._position_offset._offset_x = -32767; + detected_object._detected_object_common_data._position_offset._offset_y = -32767; + detected_object._detected_object_common_data._position_offset._offset_z = -32767; + detected_object._detected_object_common_data._pos_confidence._position_confidence = position_confidence::A_500M; + detected_object._detected_object_common_data._pos_confidence._elevation_confidence = position_confidence::A_1CM; + detected_object._detected_object_common_data._speed = 0; + detected_object._detected_object_common_data._time_measurement_offset = -1500; + detected_object._detected_object_common_data._time_confidence = time_confidence::TIME_000_000_000_000_01; + // Add Detected VRU Optional data + detected_obstacle_data detected_obstacle; + detected_obstacle._size._height = 1023; + detected_obstacle._size._width = 1023; + detected_obstacle._size._length = 1023; + + obstacle_size_confidence _size_confidence; + detected_obstacle._size_confidence._height_confidence = size_value_confidence::SIZE_0_01; + detected_obstacle._size_confidence._width_confidence = size_value_confidence::SIZE_0_01; + detected_obstacle._size_confidence._length_confidence = size_value_confidence::SIZE_0_01; + + + detected_object._detected_object_optional_data = detected_obstacle; + msg._objects.push_back(detected_object); + // Serialize Msg + std::string json = to_json(msg); + // String is not empty + EXPECT_FALSE(json.empty()); + // Confirm result has msg information in desired structure + auto result = parse_json(json); + + // Confirm timestamp data, Should fail at assert statements since if this require property is + // not available the other calls will fail + ASSERT_TRUE(result.HasMember("sdsm_time_stamp")); + ASSERT_TRUE(result.FindMember("sdsm_time_stamp")->value.IsObject()); + EXPECT_EQ( msg._time_stamp.second, result["sdsm_time_stamp"]["second"].GetInt()); + EXPECT_EQ( msg._time_stamp.minute, result["sdsm_time_stamp"]["minute"].GetInt()); + EXPECT_EQ( msg._time_stamp.hour, result["sdsm_time_stamp"]["hour"].GetInt()); + EXPECT_EQ( msg._time_stamp.day, result["sdsm_time_stamp"]["day"].GetInt()); + EXPECT_EQ( msg._time_stamp.month, result["sdsm_time_stamp"]["month"].GetInt()); + EXPECT_EQ( msg._time_stamp.year, result["sdsm_time_stamp"]["year"].GetInt()); + + EXPECT_EQ( msg._msg_count, result["msg_cnt"].GetUint()); + EXPECT_EQ( msg._source_id, result["source_id"].GetString()); + EXPECT_EQ( static_cast(msg._equipment_type), result["equipment_type"].GetInt()); + // Confirm ref position confidence + ASSERT_TRUE(result.HasMember("ref_pos_xy_conf")); + ASSERT_TRUE(result.FindMember("ref_pos_xy_conf")->value.IsObject()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_accuracy, result["ref_pos_xy_conf"]["semi_major"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_minor_axis_accuracy, result["ref_pos_xy_conf"]["semi_minor"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_orientation, result["ref_pos_xy_conf"]["orientation"].GetInt()); + // Confirm optional elevation parameter is not present + EXPECT_FALSE( result.HasMember("ref_pos_el_conf")); + // Confirm ref position + ASSERT_TRUE(result.HasMember("ref_pos")); + ASSERT_TRUE(result.FindMember("ref_pos")->value.IsObject()); + EXPECT_EQ( msg._ref_positon._longitude, result["ref_pos"]["long"].GetInt64()); + EXPECT_EQ( msg._ref_positon._latitude, result["ref_pos"]["lat"].GetInt64()); + // Optional parameter is not present + EXPECT_FALSE(result["ref_pos"].HasMember("elevation")); + + // Confirm List of detected object exists + ASSERT_TRUE(result.HasMember("objects")); + ASSERT_TRUE(result.FindMember("objects")->value.IsArray()); + ASSERT_EQ(1,result.FindMember("objects")->value.GetArray().Size()); + ASSERT_TRUE(result["objects"].GetArray()[0].IsObject()); + // Confirm object properties + auto object = result["objects"].GetArray()[0].GetObject()["detected_object_data"].GetObject(); + // Assert Object has common data + ASSERT_TRUE(object.HasMember("detected_object_common_data")); + ASSERT_TRUE(object.FindMember("detected_object_common_data")->value.IsObject()); + // Retreive Object common data + auto object_common_data = object["detected_object_common_data"].GetObject(); + // Retreive Object common data from + auto msg_object_common_data = msg._objects[0]._detected_object_common_data; + // Confirm Object common data properties + EXPECT_EQ(static_cast(msg_object_common_data._object_type), object_common_data["obj_type"].GetUint()); + EXPECT_EQ(msg_object_common_data._classification_confidence, object_common_data["obj_type_cfd"].GetUint()); + EXPECT_EQ(msg_object_common_data._object_id, object_common_data["object_id"].GetUint()); + EXPECT_EQ(msg_object_common_data._time_measurement_offset, object_common_data["measurement_time"].GetInt()); + EXPECT_EQ(static_cast(msg_object_common_data._time_confidence), object_common_data["time_confidence"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed, object_common_data["speed"].GetUint()); + EXPECT_EQ(msg_object_common_data._heading, object_common_data["heading"].GetUint()); + EXPECT_EQ(static_cast(msg_object_common_data._heading_confidence), object_common_data["heading_conf"].GetUint()); + // Test Optional properties not present + EXPECT_FALSE(object_common_data.HasMember("speed_z")); + EXPECT_FALSE(object_common_data.HasMember("speed_confidence_z")); + EXPECT_FALSE(object_common_data.HasMember("accel_4_way")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_x")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_y")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_yaw")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_z")); + // Expect obstacle optional fields information + ASSERT_TRUE(object.HasMember("detected_object_optional_data")); + ASSERT_TRUE(object["detected_object_optional_data"].HasMember("detected_obstacle_data") ); + auto msg_object_optional_data = std::get(msg._objects[0]._detected_object_optional_data.value()); + auto option_obstacle_json_data = object["detected_object_optional_data"]["detected_obstacle_data"].GetObject(); + // Confirm Size + EXPECT_EQ( msg_object_optional_data._size._length, option_obstacle_json_data["obst_size"]["length"].GetUint()); + EXPECT_EQ( msg_object_optional_data._size._width, option_obstacle_json_data["obst_size"]["width"].GetUint()); + EXPECT_EQ( msg_object_optional_data._size._height, option_obstacle_json_data["obst_size"]["height"].GetUint()); + // Confirm Size confidence + EXPECT_EQ( msg_object_optional_data._size_confidence._length_confidence, size_value_confidence_from_int(option_obstacle_json_data["obst_size_confidence"]["length_confidence"].GetUint())); + EXPECT_EQ( msg_object_optional_data._size_confidence._width_confidence, size_value_confidence_from_int(option_obstacle_json_data["obst_size_confidence"]["width_confidence"].GetUint())); + EXPECT_EQ( msg_object_optional_data._size_confidence._height_confidence.value(), size_value_confidence_from_int(option_obstacle_json_data["obst_size_confidence"]["height_confidence"].GetUint())); + + + +} + +TEST(sensor_data_sharing_msg_json_serialization_test, confirm_optional_vehicle_properties) { + sensor_data_sharing_msg msg; + msg._equipment_type = equipment_type::OBU; + msg._msg_count = 1; + msg._source_id = "00000001"; + msg._ref_positon._latitude=-90000000; + msg._ref_positon._longitude=-1800000000; + msg._time_stamp.second = 0; + msg._time_stamp.minute = 0; + msg._time_stamp.hour= 0; + msg._time_stamp.day = 0; + msg._time_stamp.month = 0; + msg._time_stamp.year = 0; + msg._ref_position_confidence._semi_major_axis_accuracy = 0; + msg._ref_position_confidence._semi_minor_axis_accuracy = 0; + msg._ref_position_confidence._semi_major_axis_orientation = 0; + // Add Detected Object + detected_object_data detected_object; + detected_object._detected_object_common_data._object_id = 0; + detected_object._detected_object_common_data._object_type = object_type::VEHICLE; + detected_object._detected_object_common_data._classification_confidence = 0; + detected_object._detected_object_common_data._heading = 0; + detected_object._detected_object_common_data._heading_confidence = heading_confidence::PREC_0_0125_deg; + detected_object._detected_object_common_data._position_offset._offset_x = -32767; + detected_object._detected_object_common_data._position_offset._offset_y = -32767; + detected_object._detected_object_common_data._position_offset._offset_z = -32767; + detected_object._detected_object_common_data._pos_confidence._position_confidence = position_confidence::A_500M; + detected_object._detected_object_common_data._pos_confidence._elevation_confidence = position_confidence::A_1CM; + detected_object._detected_object_common_data._speed = 0; + detected_object._detected_object_common_data._time_measurement_offset = -1500; + detected_object._detected_object_common_data._time_confidence = time_confidence::TIME_000_000_000_000_01; + // Add detected vehicle optional data + detected_vehicle_data detected_vehicle; + vehicle_size _vehicle_size; + _vehicle_size._width = 4095; + _vehicle_size._length = 4095; + detected_vehicle._size = _vehicle_size; + detected_vehicle._vehicle_height = 127; + + vehicle_size_confidence _vehicle_size_confidence; + + _vehicle_size_confidence._height_confidence = size_value_confidence::SIZE_0_01; + _vehicle_size_confidence._width_confidence = size_value_confidence::SIZE_0_01; + _vehicle_size_confidence._length_confidence = size_value_confidence::SIZE_0_01; + detected_vehicle._size_confidence = _vehicle_size_confidence; + + detected_vehicle.exterior_lights = "11110000"; + + angular_velocity_set _angular_velocity_set; + _angular_velocity_set._pitch_rate = 32767; + _angular_velocity_set._roll_rate = 32767; + detected_vehicle._angular_velocity = _angular_velocity_set; + + angular_velocity_confidence_set _angular_velocity_confidence_set; + + _angular_velocity_confidence_set._pitch_rate_confidence = angular_velocity_confidence::DEGSEC_01; + _angular_velocity_confidence_set._roll_rate_confidence = angular_velocity_confidence::DEGSEC_01; + detected_vehicle._angular_velocity_confidence = _angular_velocity_confidence_set; + + attitude veh_attitude; + veh_attitude._pitch = 72000; + veh_attitude._roll = 14400; + veh_attitude._yaw = 14400; + detected_vehicle._veh_attitude = veh_attitude; + + attitude_confidence veh_attitude_confidence; + veh_attitude_confidence._pitch_confidence = heading_confidence::PREC_0_05_deg; + veh_attitude_confidence._roll_confidence = heading_confidence::PREC_10_deg; + veh_attitude_confidence._yaw_confidence = heading_confidence::PREC_01_deg; + detected_vehicle._attitude_confidence = veh_attitude_confidence; + + detected_vehicle._vehicle_class = 23; + detected_vehicle._classification_confidence = 101; + + detected_object._detected_object_optional_data = detected_vehicle; + msg._objects.push_back(detected_object); + // Serialize Msg + std::string json = to_json(msg); + // String is not empty + EXPECT_FALSE(json.empty()); + // Confirm result has msg information in desired structure + auto result = parse_json(json); + + // Confirm timestamp data, Should fail at assert statements since if this require property is + // not available the other calls will fail + ASSERT_TRUE(result.HasMember("sdsm_time_stamp")); + ASSERT_TRUE(result.FindMember("sdsm_time_stamp")->value.IsObject()); + EXPECT_EQ( msg._time_stamp.second, result["sdsm_time_stamp"]["second"].GetInt()); + EXPECT_EQ( msg._time_stamp.minute, result["sdsm_time_stamp"]["minute"].GetInt()); + EXPECT_EQ( msg._time_stamp.hour, result["sdsm_time_stamp"]["hour"].GetInt()); + EXPECT_EQ( msg._time_stamp.day, result["sdsm_time_stamp"]["day"].GetInt()); + EXPECT_EQ( msg._time_stamp.month, result["sdsm_time_stamp"]["month"].GetInt()); + EXPECT_EQ( msg._time_stamp.year, result["sdsm_time_stamp"]["year"].GetInt()); + + EXPECT_EQ( msg._msg_count, result["msg_cnt"].GetUint()); + EXPECT_EQ( msg._source_id, result["source_id"].GetString()); + EXPECT_EQ( static_cast(msg._equipment_type), result["equipment_type"].GetInt()); + // Confirm ref position confidence + ASSERT_TRUE(result.HasMember("ref_pos_xy_conf")); + ASSERT_TRUE(result.FindMember("ref_pos_xy_conf")->value.IsObject()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_accuracy, result["ref_pos_xy_conf"]["semi_major"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_minor_axis_accuracy, result["ref_pos_xy_conf"]["semi_minor"].GetUint()); + EXPECT_EQ(msg._ref_position_confidence._semi_major_axis_orientation, result["ref_pos_xy_conf"]["orientation"].GetInt()); + // Confirm optional elevation parameter is not present + EXPECT_FALSE( result.HasMember("ref_pos_el_conf")); + // Confirm ref position + ASSERT_TRUE(result.HasMember("ref_pos")); + ASSERT_TRUE(result.FindMember("ref_pos")->value.IsObject()); + EXPECT_EQ( msg._ref_positon._longitude, result["ref_pos"]["long"].GetInt64()); + EXPECT_EQ( msg._ref_positon._latitude, result["ref_pos"]["lat"].GetInt64()); + // Optional parameter is not present + EXPECT_FALSE(result["ref_pos"].HasMember("elevation")); + + // Confirm List of detected object exists + ASSERT_TRUE(result.HasMember("objects")); + ASSERT_TRUE(result.FindMember("objects")->value.IsArray()); + ASSERT_EQ(1,result.FindMember("objects")->value.GetArray().Size()); + ASSERT_TRUE(result["objects"].GetArray()[0].IsObject()); + // Confirm object properties + auto object = result["objects"].GetArray()[0].GetObject()["detected_object_data"].GetObject(); + // Assert Object has common data + ASSERT_TRUE(object.HasMember("detected_object_common_data")); + ASSERT_TRUE(object.FindMember("detected_object_common_data")->value.IsObject()); + // Retreive Object common data + auto object_common_data = object["detected_object_common_data"].GetObject(); + // Retreive Object common data from + auto msg_object_common_data = msg._objects[0]._detected_object_common_data; + // Confirm Object common data properties + EXPECT_EQ(static_cast(msg_object_common_data._object_type), object_common_data["obj_type"].GetUint()); + EXPECT_EQ(msg_object_common_data._classification_confidence, object_common_data["obj_type_cfd"].GetUint()); + EXPECT_EQ(msg_object_common_data._object_id, object_common_data["object_id"].GetUint()); + EXPECT_EQ(msg_object_common_data._time_measurement_offset, object_common_data["measurement_time"].GetInt()); + EXPECT_EQ(static_cast(msg_object_common_data._time_confidence), object_common_data["time_confidence"].GetUint()); + EXPECT_EQ(msg_object_common_data._speed, object_common_data["speed"].GetUint()); + EXPECT_EQ(msg_object_common_data._heading, object_common_data["heading"].GetUint()); + EXPECT_EQ(static_cast(msg_object_common_data._heading_confidence), object_common_data["heading_conf"].GetUint()); + // Test Optional properties not present + EXPECT_FALSE(object_common_data.HasMember("speed_z")); + EXPECT_FALSE(object_common_data.HasMember("speed_confidence_z")); + EXPECT_FALSE(object_common_data.HasMember("accel_4_way")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_x")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_y")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_yaw")); + EXPECT_FALSE(object_common_data.HasMember("acc_cfd_z")); + // Expect vehicle optional fields information + ASSERT_TRUE(object.HasMember("detected_object_optional_data")); + ASSERT_TRUE(object["detected_object_optional_data"].HasMember("detected_vehicle_data") ); + auto msg_object_optional_data = std::get(msg._objects[0]._detected_object_optional_data.value()); + auto option_vehicle_json_data = object["detected_object_optional_data"]["detected_vehicle_data"].GetObject(); + + EXPECT_EQ( msg_object_optional_data.exterior_lights, option_vehicle_json_data["lights"].GetString()); + // Confirm angular velocity + EXPECT_EQ( msg_object_optional_data._angular_velocity.value()._pitch_rate, option_vehicle_json_data["veh_ang_vel"]["pitch_rate"].GetInt()); + EXPECT_EQ( msg_object_optional_data._angular_velocity.value()._roll_rate, option_vehicle_json_data["veh_ang_vel"]["roll_rate"].GetInt()); + // Confirm attitude + EXPECT_EQ( msg_object_optional_data._veh_attitude.value()._pitch, option_vehicle_json_data["veh_attitude"]["pitch"].GetInt()); + EXPECT_EQ( msg_object_optional_data._veh_attitude.value()._roll, option_vehicle_json_data["veh_attitude"]["roll"].GetInt()); + EXPECT_EQ( msg_object_optional_data._veh_attitude.value()._yaw, option_vehicle_json_data["veh_attitude"]["yaw"].GetInt()); + // Confirm size + EXPECT_EQ( msg_object_optional_data._size.value()._length, option_vehicle_json_data["size"]["length"].GetUint()); + EXPECT_EQ( msg_object_optional_data._size.value()._width, option_vehicle_json_data["size"]["width"].GetUint()); + EXPECT_EQ( msg_object_optional_data._vehicle_height, option_vehicle_json_data["height"].GetUint()); + // Confirm angular velocity confidence + EXPECT_EQ( msg_object_optional_data._angular_velocity_confidence.value()._pitch_rate_confidence, angular_velocity_confidence_from_int(option_vehicle_json_data["veh_ang_vel_confidence"]["pitch_rate_confidence"].GetUint())); + EXPECT_EQ( msg_object_optional_data._angular_velocity_confidence.value()._roll_rate_confidence, angular_velocity_confidence_from_int(option_vehicle_json_data["veh_ang_vel_confidence"]["roll_rate_confidence"].GetUint())); + // Confirm attitude confidence + EXPECT_EQ( msg_object_optional_data._attitude_confidence.value()._pitch_confidence, heading_confidence_from_int(option_vehicle_json_data["veh_attitude_confidence"]["pitch_confidence"].GetUint())); + EXPECT_EQ( msg_object_optional_data._attitude_confidence.value()._roll_confidence, heading_confidence_from_int(option_vehicle_json_data["veh_attitude_confidence"]["roll_confidence"].GetUint())); + EXPECT_EQ( msg_object_optional_data._attitude_confidence.value()._yaw_confidence, heading_confidence_from_int(option_vehicle_json_data["veh_attitude_confidence"]["yaw_confidence"].GetUint())); + // Confirm size confidence + EXPECT_EQ( msg_object_optional_data._size_confidence.value()._length_confidence, size_value_confidence_from_int(option_vehicle_json_data["vehicle_size_confidence"]["vehicle_length_confidence"].GetUint())); + EXPECT_EQ( msg_object_optional_data._size_confidence.value()._width_confidence, size_value_confidence_from_int(option_vehicle_json_data["vehicle_size_confidence"]["vehicle_width_confidence"].GetUint())); + EXPECT_EQ( msg_object_optional_data._size_confidence.value()._height_confidence, size_value_confidence_from_int(option_vehicle_json_data["vehicle_size_confidence"]["vehicle_height_confidence"].GetUint())); + // Confirm vehicle class + EXPECT_EQ( msg_object_optional_data._vehicle_class, option_vehicle_json_data["vehicle_class"].GetUint()); + // Confirm classification confidence + EXPECT_EQ( msg_object_optional_data._classification_confidence, option_vehicle_json_data["vehicle_class_conf"].GetUint()); + +} diff --git a/streets_utils/streets_phase_control_schedule/CMakeLists.txt b/streets_utils/streets_phase_control_schedule/CMakeLists.txt new file mode 100644 index 000000000..570684b68 --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.10.2) +project(streets_phase_control_schedule) +######################################################## +# Find Dependent Packages and set configurations +######################################################## +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +find_package(spdlog REQUIRED) +find_package(GTest REQUIRED CONFIG) +add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) +find_package(RapidJSON REQUIRED) +# Add definition for rapidjson to include std::string +add_definitions(-DRAPIDJSON_HAS_STDSTRING=1) + +######################################################## +# Build Library +######################################################## +file(GLOB SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/**/*.cpp) +add_library(${PROJECT_NAME}_lib ${SOURCES} ) +target_include_directories(${PROJECT_NAME}_lib + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) +target_link_libraries( ${PROJECT_NAME}_lib + PUBLIC + spdlog::spdlog + rapidjson +) + +######################################################## +# Install streets_phase_control_schedule package. +######################################################## +file(GLOB files ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h) +file(GLOB templates ${CMAKE_CURRENT_SOURCE_DIR}/include/internal/*.tpp) +install( + TARGETS ${PROJECT_NAME}_lib + EXPORT ${PROJECT_NAME}_libTargets + LIBRARY DESTINATION lib + INCLUDES DESTINATION include + ARCHIVE DESTINATION lib +) +install( + EXPORT ${PROJECT_NAME}_libTargets + FILE ${PROJECT_NAME}_libTargets.cmake + DESTINATION lib/cmake/${PROJECT_NAME}_lib/ + NAMESPACE ${PROJECT_NAME}_lib:: +) +include(CMakePackageConfigHelpers) +configure_package_config_file( + cmake/${PROJECT_NAME}_libConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_libConfig.cmake + INSTALL_DESTINATION lib/${PROJECT_NAME}_lib/${PROJECT_NAME}_lib/ ) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_libConfig.cmake + DESTINATION lib/cmake/${PROJECT_NAME}_lib/ +) +install(FILES ${files} DESTINATION include) + +######################## +# googletest for unit testing +######################## +set(BINARY ${PROJECT_NAME}_test) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) +set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +add_executable(${BINARY} ${TEST_SOURCES}) +add_test(NAME ${BINARY} COMMAND ${BINARY}) +target_include_directories(${BINARY} PUBLIC ${PROJECT_SOURCE_DIR}/include) +target_link_libraries(${BINARY} + PUBLIC + ${PROJECT_NAME}_lib +) + +if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.20") + target_link_libraries( ${BINARY} PUBLIC GTest::gtest ) +else() + target_link_libraries( ${BINARY} PUBLIC gtest ) +endif() \ No newline at end of file diff --git a/streets_utils/streets_phase_control_schedule/README.md b/streets_utils/streets_phase_control_schedule/README.md new file mode 100644 index 000000000..d6711e202 --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/README.md @@ -0,0 +1,48 @@ +# Streets Phase Control Schedule Command library +## Introduction +This CARMA-streets library is meant to handle JSON deserialization for external phase control schedule message generated by [MMITSS Roadside Processor] (https://github.com/mmitss/mmitss-az), and a sample phase control message refers to https://github.com/mmitss/mmitss-az/tree/master/src/mrp/traffic-controller-interface. + +## Phase Control Schedule +External MMITSS Roadside processor will communicate with CARMA Streets TSC service via Kafka Message Broker, and send the phase control schedule to the TSC service. Thereafter, the TSC service converts the phase control schedule to SNMP commands and communicate with Traffic Signal Controller to update the controller signal phase and schedule. +### Parameter description +| Prameter Name | Data Type | Description | +| ------------- | ------------- | ----------- | +| commandPhase | Integer | Traffic Singal Controller phase | +| commandType | Enumeration | Current supported phase control types: `omit_veh`, `omit_ped`, `hold`, `forceoff`, `call_veh`, `call_ped` | +| commandStartTime | Unsigned integer | The start epoch time in millisecond of the command execution | +| commandEndTime | Unsigned integer | The end epoch time in millisecond of the command execution | + +Note: When a clear schedule is received, the commands are cleared from the phase control schedule. +### Sample +A schedule with commands: +``` +{ + "MsgType": "Schedule", + "Schedule": [ + { + "commandEndTime": 0, + "commandPhase": 2, + "commandStartTime": 0, + "commandType": "hold" + }, + { + "commandEndTime": 69, + "commandPhase": 4, + "commandStartTime": 68, + "commandType": "forceoff" + }] +} +``` +A clear schedule: +``` +{ + "MsgType": "Schedule", + "Schedule": "Clear" +} +``` +## Including library +``` +find_package( streets_phase_control_schedule_lib REQUIRED ) +... +target_link_libraries( PUBLIC streets_phase_control_schedule_lib::streets_phase_control_schedule_lib ) +``` diff --git a/streets_utils/streets_phase_control_schedule/cmake/streets_phase_control_schedule_libConfig.cmake.in b/streets_utils/streets_phase_control_schedule/cmake/streets_phase_control_schedule_libConfig.cmake.in new file mode 100644 index 000000000..206f4cfc1 --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/cmake/streets_phase_control_schedule_libConfig.cmake.in @@ -0,0 +1,7 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(spdlog REQUIRED) +find_dependency(RapidJSON REQUIRED) + +include("${CMAKE_CURRENT_LIST_DIR}/streets_phase_control_schedule_libTargets.cmake") diff --git a/streets_utils/streets_phase_control_schedule/include/streets_phase_control_command.h b/streets_utils/streets_phase_control_schedule/include/streets_phase_control_command.h new file mode 100644 index 000000000..a768a035f --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/include/streets_phase_control_command.h @@ -0,0 +1,48 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace streets_phase_control_schedule +{ + // Define Command Actions (Used in phase controls) + enum class COMMAND_TYPE + { + CALL_VEH_PHASES = 1, // Call a vehicle phase + CALL_PED_PHASES = 2, // Call a pedestrian phase + FORCEOFF_PHASES = 3, // Forceoff a vehicle phase + HOLD_VEH_PHASES = 4, // Hold a vehicle phase + OMIT_VEH_PHASES = 5, // Omit a vehicle phase + OMIT_PED_PHASES = 6, // Omit a pedestrain phase + }; + + /** + * @brief This struct provides command information received from MMITSS. (https://github.com/mmitss/mmitss-az/tree/master/src/mrp/traffic-controller-interface). + * The command will be translated into carma-streets snmp_command structure and sent to the Traffic signal controller. + */ + struct streets_phase_control_command + { + COMMAND_TYPE command_type; // Action + int command_phase = 0; // Affected phase (bitstring to integer) + uint64_t command_start_time = 0; // Start time in epoch unix timestamp in millisecond + uint64_t command_end_time = 0; // End time in epoch unix timestamp in millisecond + streets_phase_control_command() = default; + /*** + * @brief Constructor with arguments to initialize the command object + */ + streets_phase_control_command(const std::string &command_type_str, int command_phase, uint64_t command_start_time, uint64_t command_end_time); + /** + * @brief Set the command type variable + * @param string command type in string format + */ + void set_command_type(const std::string &command_type_str); + ~streets_phase_control_command() = default; + + // Overload operator<< to print command + friend std::ostream &operator<<(std::ostream &os, const streets_phase_control_command &command); + + std::string COMMAND_TYPE_to_string(COMMAND_TYPE command_type) noexcept; + }; +} diff --git a/streets_utils/streets_phase_control_schedule/include/streets_phase_control_schedule.h b/streets_utils/streets_phase_control_schedule/include/streets_phase_control_schedule.h new file mode 100755 index 000000000..85f10ccf7 --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/include/streets_phase_control_schedule.h @@ -0,0 +1,43 @@ +#pragma once +#include "streets_phase_control_schedule_exception.h" +#include +#include +#include +#include +#include +#include +#include +#include "streets_phase_control_command.h" +#include + +namespace streets_phase_control_schedule +{ + + struct streets_phase_control_schedule + { + // When a new schedule with commands is received, the commands list is populated for schedule execution. + std::vector commands; + + /*** + * By default the indicator is set to false assumming that the initial phase control schedule does not have clear execution schedule. + * When a clear execution schedule is received, the indicator should be set to true to indicate clearing all scheduled jobs from the current phase control schedule. + * Note: A clear execution schedule has an empty list of commands. However, A schedule with an empty list of commands does not mean it is a clear schedule. Intially the schedule object has an empty list of commands. + * */ + bool is_clear_current_schedule = false; + + /** + * @brief Deserialize Phase control plan JSON into Phase control plan object. + * + * @param val Phase control plan JSON. + */ + void fromJson(const std::string &json); + /** + * @brief Transform the input value to lower case and trim the leading and tailing spaces + * @param string input value reference + */ + void toLowerCaseAndTrim(std::string &value_str) const; + + // Overload operator<< to print schedule + friend std::ostream &operator<<(std::ostream &os, const streets_phase_control_schedule& schedule); + }; +} \ No newline at end of file diff --git a/streets_utils/streets_phase_control_schedule/include/streets_phase_control_schedule_exception.h b/streets_utils/streets_phase_control_schedule/include/streets_phase_control_schedule_exception.h new file mode 100644 index 000000000..297bd7b0d --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/include/streets_phase_control_schedule_exception.h @@ -0,0 +1,23 @@ +#pragma once + +#include + + + +namespace streets_phase_control_schedule { + /** + * @brief Runtime error related to processing desired phase plan. + */ + class streets_phase_control_schedule_exception : public std::runtime_error{ + public: + /** + * @brief Destructor. + */ + ~streets_phase_control_schedule_exception() override; + /** + * @brief Constructor. + * @param msg String exception message. + */ + explicit streets_phase_control_schedule_exception(const std::string &msg ); + }; +} \ No newline at end of file diff --git a/streets_utils/streets_phase_control_schedule/src/exceptions/streets_phase_control_schedule_exception.cpp b/streets_utils/streets_phase_control_schedule/src/exceptions/streets_phase_control_schedule_exception.cpp new file mode 100644 index 000000000..8e2c68d3c --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/src/exceptions/streets_phase_control_schedule_exception.cpp @@ -0,0 +1,8 @@ +#include "streets_phase_control_schedule_exception.h" + +namespace streets_phase_control_schedule { + + streets_phase_control_schedule_exception::streets_phase_control_schedule_exception(const std::string &msg): std::runtime_error(msg){}; + + streets_phase_control_schedule_exception::~streets_phase_control_schedule_exception() = default; +} \ No newline at end of file diff --git a/streets_utils/streets_phase_control_schedule/src/models/streets_phase_control_command.cpp b/streets_utils/streets_phase_control_schedule/src/models/streets_phase_control_command.cpp new file mode 100644 index 000000000..46966d12a --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/src/models/streets_phase_control_command.cpp @@ -0,0 +1,101 @@ +#include "streets_phase_control_command.h" +#include "streets_phase_control_schedule_exception.h" + +namespace streets_phase_control_schedule +{ + streets_phase_control_command::streets_phase_control_command(const std::string &command_type_str, int phase, uint64_t start_time, uint64_t end_time) + : command_phase(phase), command_start_time(start_time), command_end_time(end_time) + { + set_command_type(command_type_str); + } + + void streets_phase_control_command::set_command_type(const std::string &command_type_str) + { + if (command_type_str == "call_veh") + { + command_type = COMMAND_TYPE::CALL_VEH_PHASES; + } + else if (command_type_str == "call_ped") + { + command_type = COMMAND_TYPE::CALL_PED_PHASES; + } + else if (command_type_str == "forceoff") + { + command_type = COMMAND_TYPE::FORCEOFF_PHASES; + } + else if (command_type_str == "hold") + { + command_type = COMMAND_TYPE::HOLD_VEH_PHASES; + } + else if (command_type_str == "omit_veh") + { + command_type = COMMAND_TYPE::OMIT_VEH_PHASES; + } + else if (command_type_str == "omit_ped") + { + command_type = COMMAND_TYPE::OMIT_PED_PHASES; + } + else + { + throw streets_phase_control_schedule::streets_phase_control_schedule_exception("Invalid command type"); + } + } + + std::ostream &operator<<(std::ostream &os, const streets_phase_control_command &command) + { + std::string command_type_str; + switch (command.command_type) + { + case COMMAND_TYPE::CALL_VEH_PHASES: + command_type_str = "call_veh"; + break; + case COMMAND_TYPE::CALL_PED_PHASES: + command_type_str = "call_ped"; + break; + case COMMAND_TYPE::FORCEOFF_PHASES: + command_type_str = "forceoff"; + break; + case COMMAND_TYPE::HOLD_VEH_PHASES: + command_type_str = "hold"; + break; + case COMMAND_TYPE::OMIT_VEH_PHASES: + command_type_str = "omit_veh"; + break; + case COMMAND_TYPE::OMIT_PED_PHASES: + command_type_str = "omit_ped"; + break; + default: + break; + } + os << "Command type: " << command_type_str << ", command phase: " << command.command_phase << ", start time: " << command.command_start_time << ", end time: " << std::setprecision(17) << command.command_end_time; + return os; + } + + std::string streets_phase_control_command::COMMAND_TYPE_to_string( COMMAND_TYPE command_type) noexcept + { + std::string command_type_str; + switch (command_type) + { + case streets_phase_control_schedule::COMMAND_TYPE::CALL_VEH_PHASES: + command_type_str = "call_veh"; + break; + case streets_phase_control_schedule::COMMAND_TYPE::CALL_PED_PHASES: + command_type_str = "call_ped"; + break; + case streets_phase_control_schedule::COMMAND_TYPE::FORCEOFF_PHASES: + command_type_str = "forceoff"; + break; + case streets_phase_control_schedule::COMMAND_TYPE::HOLD_VEH_PHASES: + command_type_str = "hold"; + break; + case streets_phase_control_schedule::COMMAND_TYPE::OMIT_VEH_PHASES: + command_type_str = "omit_veh"; + break; + case streets_phase_control_schedule::COMMAND_TYPE::OMIT_PED_PHASES: + command_type_str = "omit_ped"; + break; + } + return command_type_str; + } + +} \ No newline at end of file diff --git a/streets_utils/streets_phase_control_schedule/src/models/streets_phase_control_schedule.cpp b/streets_utils/streets_phase_control_schedule/src/models/streets_phase_control_schedule.cpp new file mode 100755 index 000000000..b9e12bd38 --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/src/models/streets_phase_control_schedule.cpp @@ -0,0 +1,108 @@ +#include "streets_phase_control_schedule.h" +#include + +namespace streets_phase_control_schedule +{ + void streets_phase_control_schedule::fromJson(const std::string &json) + { + rapidjson::Document doc; + doc.Parse(json); + if (doc.HasParseError()) + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message JSON is misformatted. JSON parsing failed!"); + } + + if (doc.HasMember("MsgType") && doc.FindMember("MsgType")->value.IsString()) + { + std::string value = doc["MsgType"].GetString(); + toLowerCaseAndTrim(value); + if (value != "schedule") + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message is required MsgType property value (=" + value + ") is not a schedule!"); + } + } + else + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message is missing required MsgType property!"); + } + + if (doc.HasMember("Schedule") && doc.FindMember("Schedule")->value.IsArray()) + { + std::vector cmd_v; + // Schedule consists of an array of commands + for (const auto &command_itr : doc["Schedule"].GetArray()) + { + // Each command requires the four properties + if (!command_itr.HasMember("commandType") || !command_itr.HasMember("commandPhase") || !command_itr.HasMember("commandStartTime") || !command_itr.HasMember("commandEndTime")) + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message is missing required commandType, commandPhase, commandStartTime or commandEndTime property!"); + } + + // Each command property has to be correct data type + if (!command_itr.FindMember("commandType")->value.IsString() || !command_itr.FindMember("commandPhase")->value.IsInt() || !command_itr.FindMember("commandStartTime")->value.IsUint64() || !command_itr.FindMember("commandEndTime")->value.IsUint64()) + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message commandType, commandPhase, commandStartTime or commandEndTime property has incorrect data type."); + } + + std::string command_type_str = command_itr["commandType"].GetString(); + toLowerCaseAndTrim(command_type_str); + if(command_itr["commandStartTime"].GetUint64() > command_itr["commandEndTime"].GetUint64()) + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message commandStartTime value cannot be greater than commandEndTime value."); + } + streets_phase_control_command command(command_type_str, command_itr["commandPhase"].GetInt(), command_itr["commandStartTime"].GetUint64(), command_itr["commandEndTime"].GetUint64()); + cmd_v.push_back(command); + } + //Display a warning message when receiving two schedules with commands, and there are no clear schedule in between. + if (!commands.empty()) + { + SPDLOG_WARN("Current schedule has commands execution, but new schedule is not a clear schedule!"); + } + // Always replacing the schedule commands with the latest new schedule. + commands = cmd_v; + // Make sure clear schedule indicator is false when there are commands in schedule + is_clear_current_schedule = false; + } + else if (doc.HasMember("Schedule") && doc.FindMember("Schedule")->value.IsString()) + { + std::string value = doc["Schedule"].GetString(); + toLowerCaseAndTrim(value); + if (value == "clear") + { + // Clear the commands from the schedule + commands.clear(); + is_clear_current_schedule = true; + } + else + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message is Schedule property has invalid value (=" + value + ") !"); + } + } + else + { + throw streets_phase_control_schedule_exception("streets_phase_control_schedule message is missing required Schedule property, or schedule value is neither string nor array!"); + } + } + + void streets_phase_control_schedule::toLowerCaseAndTrim(std::string &value_str) const + { + boost::to_lower(value_str); + boost::algorithm::trim(value_str); + } + + std::ostream &operator<<(std::ostream &os, const streets_phase_control_schedule& schedule) + { + os << "Clear status: " << (schedule.is_clear_current_schedule ? "True" : "False"); + if (!schedule.is_clear_current_schedule) + { + os << ", commands ["; + for (const auto &command : schedule.commands) + { + os << "{" << command << "},"; + } + os << "]"; + } + return os; + } + +} \ No newline at end of file diff --git a/streets_utils/streets_phase_control_schedule/test/streets_phase_control_command_test.cpp b/streets_utils/streets_phase_control_schedule/test/streets_phase_control_command_test.cpp new file mode 100644 index 000000000..ae6017a6e --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/test/streets_phase_control_command_test.cpp @@ -0,0 +1,105 @@ + +#include "streets_phase_control_command.h" +#include "streets_phase_control_schedule_exception.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace streets_phase_control_schedule; + +namespace +{ + + class streets_phase_control_command_test : public ::testing::Test + { + }; +}; + +TEST_F(streets_phase_control_command_test, set_command_type) +{ + streets_phase_control_command command1; + std::stringstream out; + std::string expected_str; + command1.set_command_type("call_veh"); + ASSERT_EQ(COMMAND_TYPE::CALL_VEH_PHASES, command1.command_type); + out.str(""); + out << command1; + expected_str = "Command type: call_veh, command phase: 0, start time: 0, end time: 0"; + ASSERT_EQ(expected_str, out.str()); + + command1.set_command_type("call_ped"); + ASSERT_EQ(COMMAND_TYPE::CALL_PED_PHASES, command1.command_type); + out.str(""); + out << command1; + expected_str = "Command type: call_ped, command phase: 0, start time: 0, end time: 0"; + ASSERT_EQ(expected_str, out.str()); + + command1.set_command_type("forceoff"); + ASSERT_EQ(COMMAND_TYPE::FORCEOFF_PHASES, command1.command_type); + out.str(""); + out << command1; + expected_str = "Command type: forceoff, command phase: 0, start time: 0, end time: 0"; + ASSERT_EQ(expected_str, out.str()); + + command1.set_command_type("hold"); + ASSERT_EQ(COMMAND_TYPE::HOLD_VEH_PHASES, command1.command_type); + out.str(""); + out << command1; + expected_str = "Command type: hold, command phase: 0, start time: 0, end time: 0"; + ASSERT_EQ(expected_str, out.str()); + + command1.set_command_type("omit_veh"); + ASSERT_EQ(COMMAND_TYPE::OMIT_VEH_PHASES, command1.command_type); + out.str(""); + out << command1; + expected_str = "Command type: omit_veh, command phase: 0, start time: 0, end time: 0"; + ASSERT_EQ(expected_str, out.str()); + + command1.set_command_type("omit_ped"); + ASSERT_EQ(COMMAND_TYPE::OMIT_PED_PHASES, command1.command_type); + out.str(""); + out << command1; + expected_str = "Command type: omit_ped, command phase: 0, start time: 0, end time: 0"; + ASSERT_EQ(expected_str, out.str()); + + ASSERT_THROW(command1.set_command_type("unkown_command"), streets_phase_control_schedule_exception); + + streets_phase_control_command command2("call_veh", 2, 0.0, 23.999767065048218); + ASSERT_EQ(COMMAND_TYPE::CALL_VEH_PHASES, command2.command_type); + expected_str = "Command type: call_veh, command phase: 2, start time: 0, end time: 23"; + out.str(""); + out << command2; + ASSERT_EQ(expected_str, out.str()); +} + +TEST_F(streets_phase_control_command_test, COMMAND_TYPE_to_string) +{ + streets_phase_control_command command1; + std::string expected_str = "call_veh"; + ASSERT_EQ(expected_str, command1.COMMAND_TYPE_to_string(COMMAND_TYPE::CALL_VEH_PHASES)); + + expected_str = "call_ped"; + ASSERT_EQ(expected_str, command1.COMMAND_TYPE_to_string(COMMAND_TYPE::CALL_PED_PHASES)); + + expected_str = "forceoff"; + ASSERT_EQ(expected_str, command1.COMMAND_TYPE_to_string(COMMAND_TYPE::FORCEOFF_PHASES)); + + expected_str = "hold"; + ASSERT_EQ(expected_str, command1.COMMAND_TYPE_to_string(COMMAND_TYPE::HOLD_VEH_PHASES)); + + expected_str = "omit_veh"; + ASSERT_EQ(expected_str, command1.COMMAND_TYPE_to_string(COMMAND_TYPE::OMIT_VEH_PHASES)); + + expected_str = "omit_ped"; + ASSERT_EQ(expected_str, command1.COMMAND_TYPE_to_string(COMMAND_TYPE::OMIT_PED_PHASES)); + + expected_str = "call_veh"; + ASSERT_EQ(expected_str, command1.COMMAND_TYPE_to_string(COMMAND_TYPE::CALL_VEH_PHASES)); +} \ No newline at end of file diff --git a/streets_utils/streets_phase_control_schedule/test/streets_phase_control_schedule_test.cpp b/streets_utils/streets_phase_control_schedule/test/streets_phase_control_schedule_test.cpp new file mode 100644 index 000000000..42b8075cf --- /dev/null +++ b/streets_utils/streets_phase_control_schedule/test/streets_phase_control_schedule_test.cpp @@ -0,0 +1,158 @@ + +#include "streets_phase_control_schedule.h" +#include "streets_phase_control_schedule_exception.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace streets_phase_control_schedule; + +namespace +{ + + class streets_phase_control_schedule_test : public ::testing::Test + { + /** + * @brief Test Setup method run before each test. + */ + void SetUp() override + { + } + /** + * @brief Test TearDown method run after each test. + * + */ + void TearDown() override + { + } + }; +}; + +TEST_F(streets_phase_control_schedule_test, fromJson) +{ + std::stringstream out; + std::string expected_str; + streets_phase_control_schedule::streets_phase_control_schedule invalid_schedule; + std::string input_str = "invalid"; + ASSERT_THROW(invalid_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + + input_str = "{\"InvalidKey\":\"Invalid\"}"; + ASSERT_THROW(invalid_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + + input_str = "{\"MsgType\":\"Invalid\",\"Schedule\":\"Clear\"}"; + ASSERT_THROW(invalid_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":\"invalid\"}"; + ASSERT_THROW(invalid_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + + streets_phase_control_schedule::streets_phase_control_schedule clear_schedule; + ASSERT_FALSE(clear_schedule.is_clear_current_schedule); + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":\"Clear\"}"; + clear_schedule.fromJson(input_str); + ASSERT_TRUE(clear_schedule.is_clear_current_schedule); + ASSERT_EQ(0, clear_schedule.commands.size()); + ASSERT_TRUE(clear_schedule.is_clear_current_schedule); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\": [{\"commandEndTime\":0}]}"; + ASSERT_THROW(invalid_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\": 0}"; + ASSERT_THROW(invalid_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + + input_str = "{\"MsgType\":\"Schedule\",\"No Schedule\": []}"; + ASSERT_THROW(invalid_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + + streets_phase_control_schedule::streets_phase_control_schedule new_schedule_wrong_data_type; + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0.0,\"commandType\":\"hold\"}]}"; + ASSERT_THROW(new_schedule_wrong_data_type.fromJson(input_str), streets_phase_control_schedule_exception); + ASSERT_FALSE(new_schedule_wrong_data_type.is_clear_current_schedule); + ASSERT_EQ(0, new_schedule_wrong_data_type.commands.size()); + + streets_phase_control_schedule::streets_phase_control_schedule new_schedule; + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"hold\"},{\"commandEndTime\":24,\"commandPhase\":4,\"commandStartTime\":5,\"commandType\":\"hold\"},{\"commandEndTime\":44,\"commandPhase\":2,\"commandStartTime\":29,\"commandType\":\"hold\"},{\"commandEndTime\":64,\"commandPhase\":4,\"commandStartTime\":49,\"commandType\":\"hold\"},{\"commandEndTime\":12,\"commandPhase\":2,\"commandStartTime\":11,\"commandType\":\"forceoff\"},{\"commandEndTime\":69,\"commandPhase\":4,\"commandStartTime\":68,\"commandType\":\"forceoff\"},{\"commandEndTime\":109,\"commandPhase\":2,\"commandStartTime\":108,\"commandType\":\"forceoff\"},{\"commandEndTime\":129,\"commandPhase\":4,\"commandStartTime\":128,\"commandType\":\"forceoff\"},{\"commandEndTime\":23,\"commandPhase\":4,\"commandStartTime\":0,\"commandType\":\"call_veh\"},{\"commandEndTime\":133,\"commandPhase\":1,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":133,\"commandPhase\":3,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":0,\"commandPhase\":6,\"commandStartTime\":0,\"commandType\":\"hold\"},{\"commandEndTime\":24,\"commandPhase\":7,\"commandStartTime\":5,\"commandType\":\"hold\"},{\"commandEndTime\":44,\"commandPhase\":6,\"commandStartTime\":29,\"commandType\":\"hold\"},{\"commandEndTime\":64,\"commandPhase\":7,\"commandStartTime\":49,\"commandType\":\"hold\"},{\"commandEndTime\":12,\"commandPhase\":6,\"commandStartTime\":11,\"commandType\":\"forceoff\"},{\"commandEndTime\":69,\"commandPhase\":7,\"commandStartTime\":68,\"commandType\":\"forceoff\"},{\"commandEndTime\":109,\"commandPhase\":6,\"commandStartTime\":108,\"commandType\":\"forceoff\"},{\"commandEndTime\":129,\"commandPhase\":7,\"commandStartTime\":128,\"commandType\":\"forceoff\"},{\"commandEndTime\":23,\"commandPhase\":7,\"commandStartTime\":0,\"commandType\":\"call_veh\"},{\"commandEndTime\":133,\"commandPhase\":5,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":133,\"commandPhase\":8,\"commandStartTime\":0,\"commandType\":\"omit_veh\"}]}"; + new_schedule.fromJson(input_str); + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + ASSERT_EQ(22, new_schedule.commands.size()); + + std::string input_str_wrong_data_type = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"hold\"},{\"commandEndTime\":0.0,\"commandPhase\":2,\"commandStartTime\":0.0,\"commandType\":\"hold\"}]}"; + ASSERT_THROW(new_schedule.fromJson(input_str_wrong_data_type), streets_phase_control_schedule_exception); + ASSERT_EQ(22, new_schedule.commands.size()); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"hold\"}]}"; + new_schedule.fromJson(input_str); + ASSERT_EQ(1, new_schedule.commands.size()); + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + expected_str = "Clear status: False, commands [{Command type: hold, command phase: 2, start time: 0, end time: 0},]"; + out.str(""); + out << new_schedule; + ASSERT_EQ(expected_str, out.str()); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"call_ped\"}]}"; + new_schedule.fromJson(input_str); + ASSERT_EQ(1, new_schedule.commands.size()); + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + expected_str = "Clear status: False, commands [{Command type: call_ped, command phase: 2, start time: 0, end time: 0},]"; + out.str(""); + out << new_schedule; + ASSERT_EQ(expected_str, out.str()); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"forceoff\"}]}"; + new_schedule.fromJson(input_str); + ASSERT_EQ(1, new_schedule.commands.size()); + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + expected_str = "Clear status: False, commands [{Command type: forceoff, command phase: 2, start time: 0, end time: 0},]"; + out.str(""); + out << new_schedule; + ASSERT_EQ(expected_str, out.str()); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"omit_veh\"}]}"; + new_schedule.fromJson(input_str); + ASSERT_EQ(1, new_schedule.commands.size()); + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + expected_str = "Clear status: False, commands [{Command type: omit_veh, command phase: 2, start time: 0, end time: 0},]"; + out.str(""); + out << new_schedule; + ASSERT_EQ(expected_str, out.str()); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"omit_ped\"}]}"; + new_schedule.fromJson(input_str); + ASSERT_EQ(1, new_schedule.commands.size()); + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + expected_str = "Clear status: False, commands [{Command type: omit_ped, command phase: 2, start time: 0, end time: 0},]"; + out.str(""); + out << new_schedule; + ASSERT_EQ(expected_str, out.str()); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":1,\"commandType\":\"omit_ped\"}]}"; + ASSERT_THROW(new_schedule.fromJson(input_str), streets_phase_control_schedule_exception); + //If exception throw, the schedule commands are not updated. + ASSERT_EQ(1, new_schedule.commands.size()); + ASSERT_FALSE(new_schedule.is_clear_current_schedule); + expected_str = "Clear status: False, commands [{Command type: omit_ped, command phase: 2, start time: 0, end time: 0},]"; + out.str(""); + out << new_schedule; + ASSERT_EQ(expected_str, out.str()); + + input_str = "{\"MsgType\":\"Schedule\",\"Schedule\":\"Clear\"}"; + new_schedule.fromJson(input_str); + ASSERT_EQ(0, new_schedule.commands.size()); + ASSERT_TRUE(new_schedule.is_clear_current_schedule); + + ASSERT_THROW(new_schedule.fromJson(input_str_wrong_data_type), streets_phase_control_schedule_exception); + ASSERT_EQ(0, new_schedule.commands.size()); + ASSERT_TRUE(new_schedule.is_clear_current_schedule); + + expected_str = "Clear status: True"; + out.str(""); + out << new_schedule; + ASSERT_EQ(expected_str, out.str()); +} \ No newline at end of file diff --git a/streets_utils/streets_service_base/test/test_main.cpp b/streets_utils/streets_phase_control_schedule/test/test_main.cpp similarity index 100% rename from streets_utils/streets_service_base/test/test_main.cpp rename to streets_utils/streets_phase_control_schedule/test/test_main.cpp diff --git a/streets_utils/streets_service_base/CMakeLists.txt b/streets_utils/streets_service_base/CMakeLists.txt index e4afbee72..a5d9757a8 100644 --- a/streets_utils/streets_service_base/CMakeLists.txt +++ b/streets_utils/streets_service_base/CMakeLists.txt @@ -72,20 +72,19 @@ install(FILES ${templates} DESTINATION include/internal) ######################## # googletest for unit testing ######################## -set(BINARY ${PROJECT_NAME}_test) +enable_testing() +set(TEST_EXECUTABLE ${PROJECT_NAME}_test) file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/test) -add_executable(${BINARY} ${TEST_SOURCES} ) -add_test(NAME ${BINARY} COMMAND ${BINARY}) -target_link_libraries(${BINARY} - PUBLIC + +add_executable(${TEST_EXECUTABLE} ${TEST_SOURCES} ) +target_link_libraries(${TEST_EXECUTABLE} + PRIVATE ${PROJECT_NAME}_lib - GTest::gtest + GTest::Main ) -target_include_directories(${BINARY} - PUBLIC - $ - $ - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src +include(GoogleTest) +gtest_discover_tests(${TEST_EXECUTABLE} + DISCOVERY_MODE PRE_TEST + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test ) \ No newline at end of file diff --git a/streets_utils/streets_service_base/include/streets_environment_variables.h b/streets_utils/streets_service_base/include/streets_environment_variables.h new file mode 100644 index 000000000..f798f30f4 --- /dev/null +++ b/streets_utils/streets_service_base/include/streets_environment_variables.h @@ -0,0 +1,24 @@ +#pragma once + +#include + +namespace streets_service { + + inline const std::string LOGS_DIRECTORY_ENV = "LOGS_DIRECTORY"; + + inline const std::string DEFAULT_LOGS_DIRECTORY = "../logs/"; + + inline const std::string SIMULATION_MODE_ENV = "SIMULATION_MODE"; + + inline const std::string DEFAULT_SIMULATION_MODE = "FALSE"; + + inline const std::string CONFIG_FILE_PATH_ENV = "CONFIG_FILE_PATH"; + + inline const std::string DEFAULT_CONFIG_FILE_PATH = "../manifest.json"; + + inline const std::string TIME_SYNC_TOPIC_ENV = "TIME_SYNC_TOPIC"; + + inline const std::string DEFAULT_TIME_SYNC_TOPIC = "time_sync"; + + +} \ No newline at end of file diff --git a/streets_utils/streets_service_base/include/streets_service.h b/streets_utils/streets_service_base/include/streets_service.h index 9bb9a9fd4..6cf058208 100644 --- a/streets_utils/streets_service_base/include/streets_service.h +++ b/streets_utils/streets_service_base/include/streets_service.h @@ -4,6 +4,7 @@ #include "kafka_client.h" #include "streets_clock_singleton.h" #include "time_sync_message.h" +#include "streets_environment_variables.h" #include @@ -59,7 +60,7 @@ namespace streets_service { * @param producer shared_ptr to kafka producer. * @return true if initialization is successful and false if initialization fails. */ - bool initialize_kafka_producer( const std::string &producer_topic, std::shared_ptr &producer ) const; + bool initialize_kafka_producer( const std::string &producer_topic, std::shared_ptr &producer ); /** * @brief Helper method to initialise Kafka consumer. NOTE: Will assign consumer group id as service_name. * @@ -67,14 +68,15 @@ namespace streets_service { * @param consumer shared_ptr to kafka consumer. * @return true if initialization is successful and false if initialization fails. */ - bool initialize_kafka_consumer( const std::string &consumer_topic, std::shared_ptr &consumer ) const; + bool initialize_kafka_consumer( const std::string &consumer_topic, std::shared_ptr &consumer ); /** - * @brief Returns string value of environment variable with given name. + * @brief Returns string value of environment variable with given name. If no value is found, returns default. * @param config_name name of environment variable. + * @param default_val string default value for environment variable if no value is found. * @throws runtime_error if config_name is nullptr or environment variable is not set. * @return value of environment variable. */ - std::string get_system_config(const char *config_name ) const; + std::string get_system_config(const char *config_name , const std::string &default_val) const noexcept; /** * @brief Method to consume continously consume time sync messages from Kafka topic and update * streets_clock_singleton with received data. @@ -92,19 +94,43 @@ namespace streets_service { * @return true if in simulation mode, false if in real time more */ bool is_simulation_mode() const; + /** + * @brief Method to create SPDLOG Daily rotating file logger. The logger is accessible by calling spdlog::get(name). The log + * files created by this logger will include data and time in name and will be stored in the LOGS_DIRECTORY set path. + * @param name Name of the logger + * @param extension file extension for log files created + * @param pattern pattern for output of log statments. Please review SPDLOG documentation + * (https://github.com/gabime/spdlog/wiki/3.-Custom-formatting#customizing-format-using-set_pattern). + * @param level log level to set for logger. Can be dynamically changed by getting logger and changing log level. + */ + std::shared_ptr create_daily_logger(const std::string &name, const std::string &extension = ".log", const std::string &pattern = "[%Y-%m-%d %H:%M:%S.%e] %v", + const spdlog::level::level_enum &level = spdlog::level::info ) const; + private: std::string _service_name; bool _simulation_mode; + std::unique_ptr _kafka_client; + std::shared_ptr _time_consumer; + std::string _logs_directory; + FRIEND_TEST(test_streets_service, test_consume_time_sync_message); FRIEND_TEST(test_streets_service, test_initialize_consumer); FRIEND_TEST(test_streets_service, test_initialize_producer); + FRIEND_TEST(test_streets_service, test_initialize_consumer_fail); + FRIEND_TEST(test_streets_service, test_initialize_producer_fail); FRIEND_TEST(test_streets_service, test_initialize_sim); + FRIEND_TEST(test_streets_service, test_initialize_sim_fail); FRIEND_TEST(test_streets_service, test_get_system_config); + FRIEND_TEST(test_streets_service, test_create_daily_logger); + FRIEND_TEST(test_streets_service, test_create_daily_logger_default); + FRIEND_TEST(test_streets_service, test_start); + + diff --git a/streets_utils/streets_service_base/src/streets_clock_singleton.cpp b/streets_utils/streets_service_base/src/streets_clock_singleton.cpp index 2c7d68fde..e9b119cb9 100644 --- a/streets_utils/streets_service_base/src/streets_clock_singleton.cpp +++ b/streets_utils/streets_service_base/src/streets_clock_singleton.cpp @@ -10,9 +10,9 @@ namespace streets_service { uint64_t streets_clock_singleton::time_in_ms() { auto &inst = get_singleton(); - SPDLOG_TRACE("Calling thread is waiting on streets clock."); + SPDLOG_TRACE("Calling thread is waiting on streets clock initialization."); inst.wait_for_initialization(); - SPDLOG_TRACE("Thread is released after initializeation."); + SPDLOG_TRACE("Thread is released after initialization."); return inst.nowInMilliseconds(); } diff --git a/streets_utils/streets_service_base/src/streets_service.cpp b/streets_utils/streets_service_base/src/streets_service.cpp index a82ce372e..dc27467f3 100644 --- a/streets_utils/streets_service_base/src/streets_service.cpp +++ b/streets_utils/streets_service_base/src/streets_service.cpp @@ -1,4 +1,5 @@ #include "streets_service.h" +#include namespace streets_service { @@ -11,19 +12,20 @@ namespace streets_service { bool streets_service::initialize() { try { - std::string config_file_path = get_system_config("CONFIG_FILE_PATH"); + std::string config_file_path = get_system_config(CONFIG_FILE_PATH_ENV.c_str(), DEFAULT_CONFIG_FILE_PATH); streets_configuration::create(config_file_path); - std::string sim_mode_string = get_system_config("SIMULATION_MODE"); + std::string sim_mode_string = get_system_config(SIMULATION_MODE_ENV.c_str(),DEFAULT_SIMULATION_MODE); _simulation_mode = sim_mode_string.compare("true") == 0 || sim_mode_string.compare("TRUE") == 0 ; streets_clock_singleton::create(_simulation_mode); _service_name = streets_configuration::get_service_name(); SPDLOG_INFO("Initializing {0} streets service in simulation mode : {1}!", _service_name, _simulation_mode); if ( _simulation_mode ) { - std::string time_sync_topic = get_system_config("TIME_SYNC_TOPIC"); + std::string time_sync_topic = get_system_config(TIME_SYNC_TOPIC_ENV.c_str(), DEFAULT_TIME_SYNC_TOPIC); if (!initialize_kafka_consumer(time_sync_topic, _time_consumer)){ return false; } } + _logs_directory = get_system_config(LOGS_DIRECTORY_ENV.c_str(), DEFAULT_LOGS_DIRECTORY); } catch( const streets_configuration_exception &e) { SPDLOG_ERROR("Exception occured during {0} initialization : {1}" , _service_name , e.what()); return false; @@ -36,12 +38,29 @@ namespace streets_service { } - bool streets_service::initialize_kafka_producer( const std::string &producer_topic, std::shared_ptr &producer ) const { - - auto client = std::make_unique(); + std::shared_ptr streets_service::create_daily_logger(const std::string &name, const std::string &extension, + const std::string &pattern, const spdlog::level::level_enum &level) const + { + auto logger = spdlog::daily_logger_mt( + name, // logger name + _logs_directory +name + extension, // log file name and path + 23, // hours to rotate + 59 // minutes to rotate + ); + // Only log log statement content + logger->set_pattern(pattern); + logger->set_level(level); + logger->flush_on(level); + return logger; + } + bool streets_service::initialize_kafka_producer( const std::string &producer_topic, std::shared_ptr &producer ) { + + if ( !_kafka_client ) { + _kafka_client = std::make_unique(); + } std::string bootstrap_server = streets_configuration::get_string_config("bootstrap_server"); - producer = client->create_producer(bootstrap_server, producer_topic); + producer = _kafka_client->create_producer(bootstrap_server, producer_topic); if (!producer->init()) { SPDLOG_CRITICAL("Kafka producer initialize error on topic {0}", producer_topic); @@ -50,11 +69,13 @@ namespace streets_service { SPDLOG_DEBUG("Initialized Kafka producer on topic {0}!", producer_topic); return true; } - - bool streets_service::initialize_kafka_consumer(const std::string &consumer_topic, std::shared_ptr &kafka_consumer ) const{ - auto client = std::make_unique(); + + bool streets_service::initialize_kafka_consumer(const std::string &consumer_topic, std::shared_ptr &kafka_consumer ){ + if ( !_kafka_client ) { + _kafka_client = std::make_unique(); + } std::string bootstrap_server = streets_configuration::get_string_config("bootstrap_server"); - kafka_consumer = client->create_consumer(bootstrap_server, consumer_topic, _service_name); + kafka_consumer = _kafka_client->create_consumer(bootstrap_server, consumer_topic, _service_name); if (!kafka_consumer->init()) { SPDLOG_CRITICAL("Kafka initialize error"); @@ -67,7 +88,7 @@ namespace streets_service { void streets_service::consume_time_sync_message() const { _time_consumer->subscribe(); while (_time_consumer->is_running()) - { + { const std::string payload = _time_consumer->consume(1000); if (payload.length() != 0) { @@ -76,14 +97,24 @@ namespace streets_service { simulation::time_sync_message msg; msg.fromJson(payload); streets_clock_singleton::update(msg.timestep); + // A script to validate time synchronization of tools in CDASim currently relies on the following + // log line. TODO: This line is meant to be removed in the future upon completion of this work: + // https://github.com/usdot-fhwa-stol/carma-analytics-fotda/pull/43 + if (spdlog::get_level() == spdlog::level::debug) + { + auto time_now = std::chrono::system_clock::now(); + auto epoch = time_now.time_since_epoch(); + auto milliseconds = std::chrono::duration_cast(epoch); + SPDLOG_DEBUG("Simulation Time: {0} where current system time is: {1}", msg.timestep, milliseconds.count()); + } } catch( const std::runtime_error &e) { SPDLOG_WARN( "{0} exception occured will consuming {1} msg! Skipping message!", e.what(), payload); } - + } - } + } } @@ -94,21 +125,22 @@ namespace streets_service { } } - std::string streets_service::get_system_config(const char *config_name) const { - if (config_name) { - try { - std::string config = std::getenv(config_name); - SPDLOG_DEBUG("Reading system config {0} as : {1}!", config_name, config); - return config; - } - catch(const std::logic_error &e) { - std::string config_name_str = config_name; - throw std::runtime_error("System config " + config_name_str + " not set!"); - } - } else { - throw std::runtime_error(" Systme config param name is null pointer!"); + std::string streets_service::get_system_config(const char *config_name, const std::string &default_val) const noexcept{ + // Check for config_name nullptr and use default value + if (config_name == nullptr ) { + SPDLOG_WARN("System config_name was nullptr! Using default value {0}!" , default_val); + return default_val; + } + // If std::getenv(config_name) returns value, use this value + if (const auto config = std::getenv(config_name)) { + SPDLOG_DEBUG("Reading system config {0} as : {1}!", config_name, config); + return config; + } + // If std::getenv(config_name) returns nullptr, environment variable was not set so use default + else { + SPDLOG_WARN("System config {0} was not set! Using default value {1}!" ,config_name, default_val); + return default_val; } - return ""; } std::string streets_service::get_service_name() const { @@ -118,4 +150,5 @@ namespace streets_service { bool streets_service::is_simulation_mode() const { return _simulation_mode; } + } \ No newline at end of file diff --git a/streets_utils/streets_service_base/test/test_streets_clock_singleton.cpp b/streets_utils/streets_service_base/test/test_streets_clock_singleton.cpp index fc0d903d9..79278d5f3 100644 --- a/streets_utils/streets_service_base/test/test_streets_clock_singleton.cpp +++ b/streets_utils/streets_service_base/test/test_streets_clock_singleton.cpp @@ -1,5 +1,8 @@ #include -#include "streets_clock_singleton.h" +#include +#include +#include + using namespace streets_service; using namespace std::chrono; @@ -14,48 +17,169 @@ namespace streets_service{ void TearDown() { SPDLOG_INFO("Tear Down"); } + int time_comparison_threshold_ms = 20; }; - - TEST_F( test_streets_clock, test_simulation_mode) { - SPDLOG_INFO("Creating simulation clock"); + /** + * @brief This test case is meant to execute the streets_clock_singleton functionality in simulation mode to block a single calling thread + * requesting any time before the carma-clock object has been initialized by a time sync message from simulation and correctly unblock/wake up + * the thread once the carma clock is initialized + * + */ + TEST_F( test_streets_clock, test_simulation_mode_single_thread_initialization) { streets_clock_singleton::create(true); // Providing a seed value srand((unsigned) time(NULL)); // Get a random number int random = rand(); int old_val = random; - //Initialize time at zero to avoid wait_for_initialization hang + auto start = std::chrono::system_clock::now(); + // Set at 100 ms + auto sleep_duration = 100; + // Thread to simulate first incoming time sync message + std::thread t1([sleep_duration]() { + std::this_thread::sleep_for(std::chrono::milliseconds(sleep_duration)); + streets_clock_singleton::update(0); + }); + // Calling thread + EXPECT_EQ(streets_clock_singleton::time_in_ms(), 0); + // Measure how long calling thread was blocked + auto end = std::chrono::system_clock::now(); + auto elapsed_time = end-start; + // Assume duration is sleep duration of update thread +/- 1ms + EXPECT_NEAR( sleep_duration, std::chrono::duration_cast(elapsed_time).count(), time_comparison_threshold_ms ); + t1.join(); + } + /** + * @brief This test case is meant to execute the streets_clock_singleton functionality in simulation mode to block a single calling thread + * using the sleep_until method and unblock/waking up the thread once the sleep_until value has been reached or passed. + * + */ + TEST_F( test_streets_clock, test_simulation_mode_single_thread_sleep_until) { + streets_clock_singleton::create(true); + // Initialize streets_clock_singleton to initial value streets_clock_singleton::update(0); + // Providing a seed value + srand((unsigned) time(NULL)); + // Get a random number + int random = rand(); + int old_val = random; + // Measure start of real time duration + auto start = std::chrono::system_clock::now(); + // Set at 100 ms + auto sleep_duration = 100; + // Thread to simulate incoming time sync update + std::thread t2([random, sleep_duration]() { + std::this_thread::sleep_for(std::chrono::milliseconds(sleep_duration)); + streets_clock_singleton::update(random); + }); + // Should block until streets_clock_singleton is updated to value. + streets_clock_singleton::sleep_until(random); + // Measure end of real time duration + auto end = std::chrono::system_clock::now(); + auto elapsed_time = end-start; + // Assume duration is sleep duration of update thread +/- 1ms + EXPECT_NEAR( sleep_duration, duration_cast(elapsed_time).count(), time_comparison_threshold_ms ); + EXPECT_EQ(streets_clock_singleton::time_in_ms(), random); + t2.join(); - ASSERT_EQ(streets_clock_singleton::time_in_ms(), 0); + }; - streets_clock_singleton::update(random); + /** + * @brief This test case is meant to execute the streets_clock_singleton functionality in simulation mode to block multiple calling threads + * requesting any time before the carma-clock object has been initialized by a time sync message and correctly unblocking/waking up all + * waiting threads once the carma clock is initialized + * + */ + TEST_F( test_streets_clock, test_simulation_mode_multiple_thread_initialization) { + streets_clock_singleton::create(true); + // Providing a seed value + srand((unsigned) time(NULL)); + // Get a random number + int random = rand(); + int old_val = random; + auto start = std::chrono::system_clock::now(); + int t1_duration_ms, t2_duration_ms, t3_duration_ms; + // Set at 100 ms + auto sleep_duration = 100; + // Thread to simulate first incoming time sync message + std::thread t1([start, &t1_duration_ms]() { + //Blocked until streets_clock_singleton is initialized with a value + streets_clock_singleton::time_in_ms(); + auto t1_end = std::chrono::system_clock::now(); + t1_duration_ms = std::chrono::duration_cast(t1_end-start).count(); - ASSERT_EQ(streets_clock_singleton::time_in_ms(), random); - // Simulate random size timestep - random += rand(); - streets_clock_singleton::update(random); + }); + std::thread t2([start, &t2_duration_ms]() { + //Blocked until streets_clock_singleton is initialized with a value + streets_clock_singleton::time_in_ms(); + auto t2_end = std::chrono::system_clock::now(); + t2_duration_ms = std::chrono::duration_cast(t2_end-start).count(); - ASSERT_EQ(streets_clock_singleton::time_in_ms(), random); - ASSERT_NE(old_val, random); - }; + }); + std::thread t3([start, &t3_duration_ms]() { + //Blocked until streets_clock_singleton is initialized with a value + streets_clock_singleton::time_in_ms(); + auto t3_end = std::chrono::system_clock::now(); + t3_duration_ms = std::chrono::duration_cast(t3_end-start).count(); + + }); + + // Simulate time sync message update + std::this_thread::sleep_for(std::chrono::milliseconds(sleep_duration)); + streets_clock_singleton::update(0); + t1.join(); + t2.join(); + t3.join(); + // Confirm update was succesful + EXPECT_EQ(streets_clock_singleton::time_in_ms(), 0); + // // Assume duration is sleep duration of update is equal to thread duration +/- 1ms + EXPECT_NEAR( sleep_duration, t1_duration_ms, time_comparison_threshold_ms ); + EXPECT_NEAR( sleep_duration, t2_duration_ms, time_comparison_threshold_ms ); + EXPECT_NEAR( sleep_duration, t3_duration_ms, time_comparison_threshold_ms ); + + } + /** + * @brief This test case is meant to test the functionality of the streets_clock_singleton to correctly handle multiple time updates + * in simulation mode. + */ + TEST_F(test_streets_clock, test_sim_mode_multiple_updates) { + streets_clock_singleton::create(true); + // Initialize streets_clock_singleton to initial value + streets_clock_singleton::update(0); + // Providing a seed value + srand((unsigned) time(NULL)); + // Get a random number + int random = rand(); + int old_val = random; + streets_clock_singleton::update(random); + EXPECT_EQ(streets_clock_singleton::time_in_ms(), random); + // Simulate random size timestep + random += rand(); + streets_clock_singleton::update(random); + EXPECT_EQ(streets_clock_singleton::time_in_ms(), random); + EXPECT_NE(old_val, random); + } + /** + * @brief This test case is to test the streets_clock_singleton functionality when not in simulation mode. When not in simulation mode the streets + * clock singleton should just return system time. + */ TEST_F(test_streets_clock, test_real_time) { streets_clock_singleton::create(false); - EXPECT_NEAR(streets_clock_singleton::time_in_ms(), duration_cast(system_clock::now().time_since_epoch()).count(), 1); + EXPECT_NEAR(streets_clock_singleton::time_in_ms(), duration_cast(system_clock::now().time_since_epoch()).count(), time_comparison_threshold_ms); // allow time to change sleep(1); - EXPECT_NEAR(streets_clock_singleton::time_in_ms(), duration_cast(system_clock::now().time_since_epoch()).count(), 1); + EXPECT_NEAR(streets_clock_singleton::time_in_ms(), duration_cast(system_clock::now().time_since_epoch()).count(), time_comparison_threshold_ms); auto cur_time = streets_clock_singleton::time_in_ms(); streets_clock_singleton::sleep_for(2000); - EXPECT_NEAR(cur_time+2000, duration_cast(system_clock::now().time_since_epoch()).count(), 1); + EXPECT_NEAR(cur_time+2000, duration_cast(system_clock::now().time_since_epoch()).count(), time_comparison_threshold_ms); cur_time = streets_clock_singleton::time_in_ms(); auto sleep_until_time = cur_time + 3000; streets_clock_singleton::sleep_until(sleep_until_time); - EXPECT_NEAR(sleep_until_time, duration_cast(system_clock::now().time_since_epoch()).count(), 1); + EXPECT_NEAR(sleep_until_time, duration_cast(system_clock::now().time_since_epoch()).count(), time_comparison_threshold_ms); } } \ No newline at end of file diff --git a/streets_utils/streets_service_base/test/test_streets_service.cpp b/streets_utils/streets_service_base/test/test_streets_service.cpp index 908024a1d..25c929649 100644 --- a/streets_utils/streets_service_base/test/test_streets_service.cpp +++ b/streets_utils/streets_service_base/test/test_streets_service.cpp @@ -1,28 +1,57 @@ #include -#include "streets_service.h" -#include "mock_kafka_consumer_worker.h" -#include "mock_kafka_producer_worker.h" +#include +#include +#include +#include +#include +#include +#include +#include using testing::_; using testing::Return; +using testing::AnyNumber; namespace streets_service{ class test_streets_service : public testing::Test { - protected: - void SetUp() { - setenv("SIMULATION_MODE", "TRUE", 1); - setenv("TIME_SYNC_TOPIC", "time_sync", 1); - setenv("CONFIG_FILE_PATH", "../test/test_files/manifest.json", 1); - } public: + std::shared_ptr mock_consumer; + std::shared_ptr mock_producer; + + void SetUp() override { + + + setenv(SIMULATION_MODE_ENV.c_str(), "TRUE", 1); + setenv(TIME_SYNC_TOPIC_ENV.c_str(), "time_sync", 1); + setenv(CONFIG_FILE_PATH_ENV.c_str(), "../test/test_files/manifest.json", 1); + setenv(LOGS_DIRECTORY_ENV.c_str(), "../logs/", 1); + } streets_service serv; }; TEST_F(test_streets_service, test_initialize_sim) { - ASSERT_TRUE(serv.initialize()); - ASSERT_EQ( serv.get_service_name(), "test_service"); - ASSERT_TRUE(serv.is_simulation_mode()); + serv._kafka_client = std::make_unique(); + mock_consumer = std::make_shared(); + + // Set mock client to return mock producer and consumer respectively on calls to create_producer and create_consumer + EXPECT_CALL(dynamic_cast(*serv._kafka_client), create_consumer("127.0.0.1:9092", "time_sync" , "test_service") ).Times(1).WillRepeatedly(Return(mock_consumer)); + EXPECT_CALL(*mock_consumer, init() ).Times(1).WillRepeatedly(Return(true)); + EXPECT_TRUE(serv.initialize()); + EXPECT_EQ( serv.get_service_name(), "test_service"); + EXPECT_TRUE(serv.is_simulation_mode()); + }; + + TEST_F(test_streets_service, test_initialize_sim_fail) { + serv._kafka_client = std::make_unique(); + mock_consumer = std::make_shared(); + + // Set mock client to return mock producer and consumer respectively on calls to create_producer and create_consumer + EXPECT_CALL(dynamic_cast(*serv._kafka_client), create_consumer("127.0.0.1:9092", "time_sync" , "test_service") ).Times(1).WillRepeatedly(Return(mock_consumer)); + EXPECT_CALL(*mock_consumer, init() ).Times(1).WillRepeatedly(Return(false)); + EXPECT_FALSE(serv.initialize()); + EXPECT_EQ( serv.get_service_name(), "test_service"); + EXPECT_TRUE(serv.is_simulation_mode()); }; TEST_F(test_streets_service, test_consume_time_sync_message) { @@ -40,49 +69,137 @@ namespace streets_service{ "\"seq\":123" "}" )); + // Create Carma Clock Singleton + streets_clock_singleton::create(true); serv.consume_time_sync_message(); // Skip empty message and skip incorrect message and consume real message then // consumer is_running returns false and returns control - ASSERT_EQ(1400, streets_clock_singleton::time_in_ms()); + EXPECT_EQ(1400, streets_clock_singleton::time_in_ms()); } + TEST_F(test_streets_service, test_create_daily_logger) { + serv.initialize(); + auto logger = serv.create_daily_logger("Test_log", ".test", "%v", spdlog::level::critical); + EXPECT_EQ(spdlog::level::critical, logger->level()); + EXPECT_EQ("Test_log", logger->name()); + std::fstream log_file; + std::string content; + std::time_t t = std::time(nullptr); + std::tm* now = std::localtime(&t); + char buffer[128]; + strftime(buffer, sizeof(buffer), "_%Y-%m-%d", now); + std::string file_path_string = "../logs/" + logger->name()+ buffer + ".test"; + log_file.open(file_path_string, std::ios::out); + EXPECT_TRUE(log_file.good()); + log_file.close(); + + } + TEST_F(test_streets_service, test_create_daily_logger_default) { + serv.initialize(); + auto logger = serv.create_daily_logger("default_daily"); + EXPECT_EQ(spdlog::level::info, logger->level()); + EXPECT_EQ("default_daily", logger->name()); + std::fstream log_file; + std::string content; + std::time_t t = std::time(nullptr); + std::tm* now = std::localtime(&t); + char buffer[128]; + strftime(buffer, sizeof(buffer), "_%Y-%m-%d", now); + std::string file_path_string = "../logs/" + logger->name()+ buffer + ".log"; + log_file.open(file_path_string, std::ios::out); + EXPECT_TRUE(log_file.good()); + log_file.close(); + } + TEST_F(test_streets_service, test_initialize_consumer) { - serv._service_name ="TestService"; + serv._service_name ="test_service"; std::shared_ptr consumer; - ASSERT_TRUE(serv.initialize_kafka_consumer("test_topic", consumer)); - consumer->stop(); + mock_consumer = std::make_shared(); + serv._kafka_client = std::make_unique(); + + // Set mock client to return mock producer and consumer respectively on calls to create_producer and create_consumer + EXPECT_CALL(dynamic_cast(*serv._kafka_client), create_consumer("127.0.0.1:9092", "test_topic" , "test_service") ).Times(1).WillRepeatedly(Return(mock_consumer)); + // Set mock client to return mock producer and consumer respectively on calls to create_producer and create_consumer + EXPECT_CALL(*mock_consumer, init() ).Times(1).WillRepeatedly(Return(true)); + // Create streets configuration singleton + streets_configuration::create("../test/test_files/manifest.json"); + EXPECT_TRUE(serv.initialize_kafka_consumer("test_topic", consumer)); + }; + +TEST_F(test_streets_service, test_initialize_consumer_fail) { + serv._service_name ="test_service"; + std::shared_ptr consumer; + mock_consumer = std::make_shared(); + serv._kafka_client = std::make_unique(); + + // Set mock client to return mock producer and consumer respectively on calls to create_producer and create_consumer + EXPECT_CALL(dynamic_cast(*serv._kafka_client), create_consumer("127.0.0.1:9092", "test_topic" , "test_service") ).Times(1).WillRepeatedly(Return(mock_consumer)); + // Set mock client to return mock producer and consumer respectively on calls to create_producer and create_consumer + EXPECT_CALL(*mock_consumer, init() ).Times(1).WillRepeatedly(Return(false)); + // Create streets configuration singleton + streets_configuration::create("../test/test_files/manifest.json"); + EXPECT_FALSE(serv.initialize_kafka_consumer("test_topic", consumer)); }; TEST_F(test_streets_service, test_initialize_producer) { - serv._service_name ="TestService"; + serv._service_name ="test_service"; + std::shared_ptr producer; + mock_producer = std::make_shared(); + serv._kafka_client = std::make_unique(); + + // Set mock client to return mock producer and producer respectively on calls to create_producer and create_producer + EXPECT_CALL(dynamic_cast(*serv._kafka_client), create_producer("127.0.0.1:9092", "test_topic") ).Times(1).WillRepeatedly(Return(mock_producer)); + // Set mock client to return mock producer and producer respectively on calls to create_producer and create_producer + EXPECT_CALL(*mock_producer, init() ).Times(1).WillRepeatedly(Return(true)); + // Create streets configuration singleton + streets_configuration::create("../test/test_files/manifest.json"); + EXPECT_TRUE(serv.initialize_kafka_producer("test_topic", producer)); + }; + + TEST_F(test_streets_service, test_initialize_producer_fail) { + serv._service_name ="test_service"; std::shared_ptr producer; - ASSERT_TRUE(serv.initialize_kafka_producer("test_topic", producer)); - producer->stop(); + mock_producer = std::make_shared(); + serv._kafka_client = std::make_unique(); + + // Set mock client to return mock producer and producer respectively on calls to create_producer and create_producer + EXPECT_CALL(dynamic_cast(*serv._kafka_client), create_producer("127.0.0.1:9092", "test_topic" ) ).Times(1).WillRepeatedly(Return(mock_producer)); + // Set mock client to return mock producer and producer respectively on calls to create_producer and create_producer + EXPECT_CALL(*mock_producer, init() ).Times(1).WillRepeatedly(Return(false)); + // Create streets configuration singleton + streets_configuration::create("../test/test_files/manifest.json"); + EXPECT_FALSE(serv.initialize_kafka_producer("test_topic", producer)); }; TEST_F(test_streets_service, test_get_system_config) { - std::string simulation_mode = serv.get_system_config("SIMULATION_MODE"); - ASSERT_EQ(simulation_mode, "TRUE"); + std::string simulation_mode = serv.get_system_config("SIMULATION_MODE", "DEFAULT"); + EXPECT_EQ(simulation_mode, "TRUE"); - ASSERT_THROW(serv.get_system_config("NON_EXISTANT"), std::runtime_error); - ASSERT_THROW(serv.get_system_config(nullptr), std::runtime_error); + EXPECT_EQ(serv.get_system_config("NON_EXISTANT", "DEFAULT"), "DEFAULT"); + EXPECT_EQ(serv.get_system_config(nullptr, "DEFAULT"), "DEFAULT"); }; TEST_F(test_streets_service, test_start) { - ASSERT_TRUE(serv.initialize()); + EXPECT_TRUE(serv.initialize()); + serv.start(); } - TEST_F(test_streets_service, test_initialize_exception) { - unsetenv("CONFIG_FILE_PATH"); - ASSERT_FALSE(serv.initialize()); + TEST_F(test_streets_service, test_initialize_defaults) { + unsetenv(SIMULATION_MODE_ENV.c_str()); + unsetenv(TIME_SYNC_TOPIC_ENV.c_str()); + // Default Config file path does not work here since there is not configuration + // file in the package directory. + // unsetenv(CONFIG_FILE_PATH_ENV.c_str()); + unsetenv(LOGS_DIRECTORY_ENV.c_str()); + EXPECT_TRUE(serv.initialize()); } TEST_F(test_streets_service, test_initialize_exception_config ) { setenv("CONFIG_FILE_PATH", "../test/test_files/invalid.json", 1); - ASSERT_FALSE(serv.initialize()); + EXPECT_FALSE(serv.initialize()); } diff --git a/streets_utils/streets_signal_optimization/CMakeLists.txt b/streets_utils/streets_signal_optimization/CMakeLists.txt index aa9b0892f..0ba39572b 100644 --- a/streets_utils/streets_signal_optimization/CMakeLists.txt +++ b/streets_utils/streets_signal_optimization/CMakeLists.txt @@ -4,6 +4,8 @@ project(streets_signal_optimization) # Find Dependent Packages and set configurations ######################################################## set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC ") +# std::shared_mutex and std::map::try_emplace +set(CMAKE_CXX_STANDARD 17) find_package(spdlog REQUIRED) add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) find_package(Boost COMPONENTS system filesystem thread REQUIRED) @@ -21,7 +23,7 @@ find_package(carma-clock REQUIRED) ######################################################## # Build Library ######################################################## -file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/ *.cpp) +file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) add_library(${PROJECT_NAME}_lib ${SOURCES} ) target_include_directories(${PROJECT_NAME}_lib PUBLIC @@ -81,9 +83,10 @@ file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) add_executable(${BINARY} ${TEST_SOURCES} ) add_test(NAME ${BINARY} COMMAND ${BINARY}) +message(status "Test sources ${TEST_SOURCES}") target_include_directories(${BINARY} PUBLIC ${PROJECT_SOURCE_DIR}/include) target_link_libraries(${BINARY} - PUBLIC + PRIVATE ${PROJECT_NAME}_lib rapidjson ) diff --git a/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_arbitrator.cpp b/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_arbitrator.cpp index 3ec5133c7..2be1651a4 100644 --- a/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_arbitrator.cpp +++ b/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_arbitrator.cpp @@ -31,9 +31,7 @@ namespace streets_signal_optimization protected: void SetUp() override { - - - + streets_service::streets_clock_singleton::create(false); tsc_state = std::make_shared(); std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::milliseconds epochMs = std::chrono::duration_cast(now.time_since_epoch()); diff --git a/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_generator.cpp b/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_generator.cpp index fbcd9555f..bbcf25985 100644 --- a/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_generator.cpp +++ b/streets_utils/streets_signal_optimization/test/test_streets_desired_phase_plan_generator.cpp @@ -69,6 +69,7 @@ TEST(test_streets_desired_phase_plan_generator, test_set_configuration) { * red-clearance for 2 seconds. */ TEST(test_streets_desired_phase_plan_generator, test_convert_spat_to_dpp) { + streets_service::streets_clock_singleton::create(false); /** * spat: @@ -276,6 +277,7 @@ TEST(test_streets_desired_phase_plan_generator, test_signal_group_entry_lane_map namespace streets_signal_optimization { TEST(test_streets_desired_phase_plan_generator, test_generate_desired_phase_plan_list) { + streets_service::streets_clock_singleton::create(false); streets_desired_phase_plan_generator generator; diff --git a/streets_utils/streets_snmp_cmd/CMakeLists.txt b/streets_utils/streets_snmp_cmd/CMakeLists.txt new file mode 100644 index 000000000..bb9d082c7 --- /dev/null +++ b/streets_utils/streets_snmp_cmd/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.10.2) +project(streets_snmp_cmd) +######################################################## +# Find Dependent Packages and set configurations +######################################################## +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +# std::map::try_emplace +set(CMAKE_CXX_STANDARD 17) +find_package(spdlog REQUIRED) +find_package(GTest REQUIRED CONFIG) +add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) +find_package(RapidJSON REQUIRED) +find_package(streets_phase_control_schedule_lib REQUIRED) +# Add definition for rapidjson to include std::string +add_definitions(-DRAPIDJSON_HAS_STDSTRING=1) + +######################################################## +# Build Library +######################################################## +file(GLOB SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/**/*.cpp) +add_library(${PROJECT_NAME}_lib ${SOURCES} ) +target_include_directories(${PROJECT_NAME}_lib + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) +target_link_libraries( ${PROJECT_NAME}_lib + PUBLIC + spdlog::spdlog + rapidjson + streets_phase_control_schedule_lib::streets_phase_control_schedule_lib +) + +######################################################## +# Install streets_snmp_cmd package. +######################################################## +file(GLOB files ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h) +file(GLOB templates ${CMAKE_CURRENT_SOURCE_DIR}/include/internal/*.tpp) +install( + TARGETS ${PROJECT_NAME}_lib + EXPORT ${PROJECT_NAME}_libTargets + LIBRARY DESTINATION lib + INCLUDES DESTINATION include + ARCHIVE DESTINATION lib +) +install( + EXPORT ${PROJECT_NAME}_libTargets + FILE ${PROJECT_NAME}_libTargets.cmake + DESTINATION lib/cmake/${PROJECT_NAME}_lib/ + NAMESPACE ${PROJECT_NAME}_lib:: +) +include(CMakePackageConfigHelpers) +configure_package_config_file( + cmake/${PROJECT_NAME}_libConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_libConfig.cmake + INSTALL_DESTINATION lib/${PROJECT_NAME}_lib/${PROJECT_NAME}_lib/ ) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_libConfig.cmake + DESTINATION lib/cmake/${PROJECT_NAME}_lib/ +) +install(FILES ${files} DESTINATION include) + +######################## +# googletest for unit testing +######################## +set(BINARY ${PROJECT_NAME}_test) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) +set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +add_executable(${BINARY} ${TEST_SOURCES}) +add_test(NAME ${BINARY} COMMAND ${BINARY}) +target_include_directories(${BINARY} PUBLIC ${PROJECT_SOURCE_DIR}/include) +target_link_libraries(${BINARY} + PUBLIC + ${PROJECT_NAME}_lib +) + +if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.20") + target_link_libraries( ${BINARY} PUBLIC GTest::gtest ) +else() + target_link_libraries( ${BINARY} PUBLIC gtest ) +endif() \ No newline at end of file diff --git a/streets_utils/streets_snmp_cmd/README.md b/streets_utils/streets_snmp_cmd/README.md new file mode 100644 index 000000000..ad8069348 --- /dev/null +++ b/streets_utils/streets_snmp_cmd/README.md @@ -0,0 +1,29 @@ +# Streets SNMP Command library +## Introduction +This CARMA-streets library is meant to define the streets SNMP command and converts external schedule into SNMP commands. + +## Streets SNMP command structure +External MMITSS Roadside processor will communicate with CARMA Streets TSC service via Kafka Message Broker, and send the phase control schedule to the TSC service. Thereafter, the TSC service converts the phase control schedule to Streets SNMP commands and communicate with Traffic Signal Controller to update the controller signal phase and schedule. +The Streets SNMP command definition: +### Parameter description +#### snmp_cmd_struct definition +| Prameter Name | Data Type | Description | +| ------------- | ------------- | ----------- | +| set_val_ | snmp_response_obj | SNMP response object populated by the SNMP response.| +| start_time_ | uint64_t | Start execution time of the SNMP command. | +| control_type_ | PHASE_CONTROL_TYPE | The type of SNMP command control sent to TSC. | + +#### snmp_response_obj definition +| Prameter Name | Data Type | Description | +| ------------- | ------------- | ----------- | +| val_int | int64_t | Integer value sent to TSC via SNMP command.| +| val_string | std::vector | String value sent to TSC via SNMP command. | +| type | RESPONSE_TYPE | The type of value being requested or set, on the TSC | + + +## Including library +``` +find_package( streets_snmp_cmd_lib REQUIRED ) +... +target_link_libraries( PUBLIC streets_snmp_cmd_lib::streets_snmp_cmd_lib ) +``` diff --git a/streets_utils/streets_snmp_cmd/cmake/streets_snmp_cmd_libConfig.cmake.in b/streets_utils/streets_snmp_cmd/cmake/streets_snmp_cmd_libConfig.cmake.in new file mode 100644 index 000000000..85e205314 --- /dev/null +++ b/streets_utils/streets_snmp_cmd/cmake/streets_snmp_cmd_libConfig.cmake.in @@ -0,0 +1,7 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(spdlog REQUIRED) +find_dependency(RapidJSON REQUIRED) + +include("${CMAKE_CURRENT_LIST_DIR}/streets_snmp_cmd_libTargets.cmake") diff --git a/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd.h b/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd.h new file mode 100644 index 000000000..91cd7285e --- /dev/null +++ b/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd.h @@ -0,0 +1,74 @@ +#pragma once +#include +#include +#include + +namespace streets_snmp_cmd +{ + enum class REQUEST_TYPE + { + GET, + SET, + OTHER // Processing this request type is not a defined behavior, included for testing only + }; + + /** @brief The type of control being set on the TSC */ + enum class PHASE_CONTROL_TYPE + { + CALL_VEH_PHASES = 1, // Call a vehicle phase + CALL_PED_PHASES = 2, // Call a pedestrian phase + FORCEOFF_PHASES = 3, // Forceoff a vehicle phase + HOLD_VEH_PHASES = 4, // Hold a vehicle phase + OMIT_VEH_PHASES = 5, // Omit a vehicle phase + OMIT_PED_PHASES = 6, // Omit a pedestrain phase + }; + + /** @brief The type of value being requested or set, on the TSC */ + enum class RESPONSE_TYPE + { + INTEGER, + STRING + }; + + /** @brief A struct to hold the value being sent to the TSC, can be integer or string. Type needs to be defined*/ + struct snmp_response_obj + { + // snmp response values can be any asn.1 supported types. + // Integer and string values can be processed here + int64_t val_int = 0; + std::vector val_string; + RESPONSE_TYPE type; + + inline bool operator==(const snmp_response_obj &obj2) const + { + return val_int == obj2.val_int && val_string == obj2.val_string && type == obj2.type; + } + }; + + /** @brief Object to store snmp control commands. Contructed with an initialized snmp_client_worker_ this object stores SNMP HOLD and OMIT commands + to be executed at specified time */ + struct snmp_cmd_struct + { + + /*Value to be set for Hold/Omit*/ + snmp_response_obj set_val_; + /*Time at which the snmp set command should be executed*/ + uint64_t start_time_; + /*Type of the snmp set command this object creates- Hold or Omit*/ + PHASE_CONTROL_TYPE control_type_; + + snmp_cmd_struct(uint64_t start_time, PHASE_CONTROL_TYPE type, int64_t val) + : start_time_(start_time), control_type_(type) + { + set_val_.type = RESPONSE_TYPE::INTEGER; + set_val_.val_int = val; + } + + /** + * @brief Method to return information about the snmp set command + * @return Returns string formatted as "control_cmd_type:;execution_start_time:;signal_groups_set:". + * */ + std::string get_cmd_info() const; + }; + +} // namespace streets_snmp_cmd diff --git a/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd_converter.h b/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd_converter.h new file mode 100644 index 000000000..12b3e7faa --- /dev/null +++ b/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd_converter.h @@ -0,0 +1,85 @@ +#pragma once +#include "streets_snmp_cmd.h" +#include "streets_phase_control_schedule.h" +#include "streets_snmp_cmd_exception.h" +#include +#include + +namespace streets_snmp_cmd +{ + class streets_snmp_cmd_converter + { + private: + /*** + * @brief Private method to support the phase control schedule conversion to snmp_cmd_struct. It takes each individual streets_phase_control_command, and populate a two dimension map with start time, command type and a vector of phases. + * Sort the streets_phase_control_command using start time, and categorize the phases based on the phase control schedule command types. + * @param streets_phase_control_command a structure of phase control command. + * @param time_cmd_m Two dimension map with key of snmp_start_time, and value of inner map. Inner map is with key of phase control schedule command type, and value of a vector of phases numbers. + * @param pcs_cmd phase control command structure that has the command start and end time, command phase and type + */ + void add_phase_control_schedule_command_to_two_dimension_map(uint64_t start_time, streets_phase_control_schedule::streets_phase_control_command pcs_cmd, std::map>> &time_cmd_m) const; + /*** + * @brief Private method to print the two dimension map with command start time, command type and a vector of phases. + * @param time_cmd_m a map of command type and the phases the command is applied to. + * @param is_cmd_start indicator whether the map of command type and phases is command sent at start time or end time from the phase control schedule. + */ + void print_two_dimension_map(std::map>> &time_cmd_m, bool is_cmd_start = true) const; + /** + * @brief Private method to perform bitwise shift or operation on input value and left shift the value by the phases number of positions. + * @param val The value sent by the SNMP command to traffic signal controller. + * @param phases The numbers of left shift positions applied to the value. + */ + uint8_t bitwise_or_phases(uint8_t val, const std::vector& phases) const; + /** + * @brief Private method to perform bitwise shift xor operation on input value and left shift the value by the phases number of positions. + * @param val The value sent by the SNMP command to traffic signal controller. + * @param phases The numbers of left shift positions applied to the value. + */ + uint8_t bitwise_xor_phases(uint8_t val, const std::vector& phases) const; + /** + * @brief Private method to update the queue with the input SNMP command. + * @param cmds_queue A queue of SNMP commands to be updated. + * @param start_time The start execution time of the command. + * @param command_type The type of command. + * @param val The value of the command to be sent to TSC. + */ + void push_snmp_command_to_queue(std::queue& cmds_queue, uint64_t start_time, PHASE_CONTROL_TYPE command_type, int64_t val) const; + + public: + streets_snmp_cmd_converter() = default; + ~streets_snmp_cmd_converter() = default; + /** + * @brief Method to create SNMP commands for provided phases and a given phase control type. + * @param phases A list of phases the SNMP command applies to. + * @param phase_control_type The type of phase control of the SNMP command. + * @param start_time Time at which the snmp command needs to be sent. + * **/ + snmp_cmd_struct create_snmp_command_by_phases(const std::vector &phases, PHASE_CONTROL_TYPE phase_control_type, int64_t start_time) const; + /*** + * @brief Method to create SNMP reset commands for all phases and a given phase control type. * + * @param phase_control_type The type of phase control of the SNMP command. + * @param start_time Time at which the snmp command needs to be sent. + */ + snmp_cmd_struct create_snmp_reset_command(PHASE_CONTROL_TYPE phase_control_type, int64_t start_time) const; + /** + * @brief Method to create SNMP commands for provided phase control schedule. + * @param phase_control_schedule The phase control schedule has a list of commands (commandPhase, commandType, commandStartTime, and commandEndTime). + */ + std::queue create_snmp_cmds_by_phase_control_schedule(const std::shared_ptr phase_control_schedule) const; + + /** + * @brief Conversion between phase control schedule command type and the SNMP command phase control type in the streets SNMP command structure. + * @param command_type an enum of command types defined in the streets phase control schedule. + */ + PHASE_CONTROL_TYPE to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE command_type) const; + + /** + * @brief Create a vector of SNMP command structures of all current supported phase control types: CALL_VEH_PHASES, CALL_PED_PHASES, FORCEOFF_PHASES, HOLD_VEH_PHASES, OMIT_VEH_PHASES, OMIT_PED_PHASES. + * All phase control OIDs are set to 0, and the commands are used to clear all the phases controls from the traffic signal. + * @param execution_time when the SNMP commands are executed. + * @return A vector of SNMP commands structures. + */ + std::vector create_clear_all_snmp_commands(uint64_t execution_time) const; + }; + +} // namespace streets_snmp_cmd diff --git a/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd_exception.h b/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd_exception.h new file mode 100644 index 000000000..baa8177db --- /dev/null +++ b/streets_utils/streets_snmp_cmd/include/streets_snmp_cmd_exception.h @@ -0,0 +1,20 @@ +#pragma once + +#include +namespace streets_snmp_cmd { + /** + * @brief Runtime error related to processing streets snmp cmd. + */ + class streets_snmp_cmd_exception : public std::runtime_error{ + public: + /** + * @brief Destructor. + */ + ~streets_snmp_cmd_exception() override; + /** + * @brief Constructor. + * @param msg String exception message. + */ + explicit streets_snmp_cmd_exception(const std::string &msg ); + }; +} \ No newline at end of file diff --git a/streets_utils/streets_snmp_cmd/src/exceptions/streets_snmp_cmd_exception.cpp b/streets_utils/streets_snmp_cmd/src/exceptions/streets_snmp_cmd_exception.cpp new file mode 100644 index 000000000..2c19b4188 --- /dev/null +++ b/streets_utils/streets_snmp_cmd/src/exceptions/streets_snmp_cmd_exception.cpp @@ -0,0 +1,8 @@ +#include "streets_snmp_cmd_exception.h" + +namespace streets_snmp_cmd { + + streets_snmp_cmd_exception::streets_snmp_cmd_exception(const std::string &msg): std::runtime_error(msg){}; + + streets_snmp_cmd_exception::~streets_snmp_cmd_exception() = default; +} \ No newline at end of file diff --git a/streets_utils/streets_snmp_cmd/src/models/streets_snmp_cmd.cpp b/streets_utils/streets_snmp_cmd/src/models/streets_snmp_cmd.cpp new file mode 100644 index 000000000..2868d6e97 --- /dev/null +++ b/streets_utils/streets_snmp_cmd/src/models/streets_snmp_cmd.cpp @@ -0,0 +1,39 @@ +#include "streets_snmp_cmd.h" + +namespace streets_snmp_cmd +{ + std::string snmp_cmd_struct::get_cmd_info() const + { + + std::string control_type_str; + switch (control_type_) + { + case PHASE_CONTROL_TYPE::HOLD_VEH_PHASES: + control_type_str = "Vehicle Hold"; + break; + case PHASE_CONTROL_TYPE::FORCEOFF_PHASES: + control_type_str = "Vehicle Forceoff"; + break; + case PHASE_CONTROL_TYPE::CALL_PED_PHASES: + control_type_str = "Pedestrian Call"; + break; + case PHASE_CONTROL_TYPE::CALL_VEH_PHASES: + control_type_str = "Vehicle Call"; + break; + case PHASE_CONTROL_TYPE::OMIT_PED_PHASES: + control_type_str = "Pedestrian Omit"; + break; + case PHASE_CONTROL_TYPE::OMIT_VEH_PHASES: + control_type_str = "Vehicle Omit"; + break; + default: + break; + } + std::string execution_start_time = std::to_string(start_time_); + + // Convert value set to phases nums + std::string value_set = std::to_string(set_val_.val_int); + + return "control_cmd_type:" + control_type_str + "; execution_start_time:" + execution_start_time + "; value_set:" + value_set; + } +} \ No newline at end of file diff --git a/streets_utils/streets_snmp_cmd/src/models/streets_snmp_cmd_converter.cpp b/streets_utils/streets_snmp_cmd/src/models/streets_snmp_cmd_converter.cpp new file mode 100644 index 000000000..681bf92bf --- /dev/null +++ b/streets_utils/streets_snmp_cmd/src/models/streets_snmp_cmd_converter.cpp @@ -0,0 +1,399 @@ +#include "streets_snmp_cmd_converter.h" +#include + +namespace streets_snmp_cmd +{ + snmp_cmd_struct streets_snmp_cmd_converter::create_snmp_command_by_phases(const std::vector &phases, PHASE_CONTROL_TYPE phase_control_type, int64_t start_time) const + { + uint8_t set_val = 0; // Initialize to 00000000 + set_val = bitwise_or_phases(set_val, phases); + snmp_cmd_struct command(start_time, phase_control_type, static_cast(set_val)); + return command; + } + + snmp_cmd_struct streets_snmp_cmd_converter::create_snmp_reset_command(PHASE_CONTROL_TYPE phase_control_type, int64_t start_time) const + { + snmp_cmd_struct command(start_time, phase_control_type, static_cast(0)); + return command; + } + + std::queue streets_snmp_cmd_converter::create_snmp_cmds_by_phase_control_schedule(const std::shared_ptr phase_control_schedule) const + { + // A queue of SNMP commands to return after processing the input phase control schedule. + std::queue cmds_result; + std::map>> start_time_cmd_m; + std::map>> end_time_cmd_m; + for (auto &pcs_cmd : phase_control_schedule->commands) + { + // Start time commands + add_phase_control_schedule_command_to_two_dimension_map(pcs_cmd.command_start_time, pcs_cmd, start_time_cmd_m); + // End time commands + add_phase_control_schedule_command_to_two_dimension_map(pcs_cmd.command_end_time, pcs_cmd, end_time_cmd_m); + } + print_two_dimension_map(start_time_cmd_m); + print_two_dimension_map(end_time_cmd_m, false); + + // Keep track of the SNMP command start time. It is used by the snmp_cmd_struct to create a streets defined SNMP command. + uint64_t snmp_cmd_start_time = 0; + // Keep track of the previous SNMP command start time. It is used to determine whether to create a separate SNMP command based on end time. + uint64_t prev_snmp_start_time = 0; + // Below values are affected by the whole phase control schedule, and it is increased or decreased by all phases in the vector of phase control schedule commands. + uint8_t set_val_hold = 0; // Initialize hold value to 00000000 + uint8_t set_val_forceoff = 0; // Initialize forceoff value to 00000000 + uint8_t set_val_call_veh = 0; // Initialize call vehicle value to 00000000 + uint8_t set_val_call_ped = 0; // Initialize call pedestrian value to 00000000 + uint8_t set_val_omit_veh = 0; // Initialize omit vehicle value to 00000000 + uint8_t set_val_omit_ped = 0; // Initialize omit pedestrian value to 00000000 + /*** + * There are two maps (start_time_cmd_m, end_time_cmd_m) to work with in the following loops. The start time commands map contains all the commands at start time. + * The end time commands map contains all the commands at end time. + * 1. Interate the start time commands map. + * ** 1.1 Iterate the end time commands map, checking if the end time command is executed before the current start time command, and after the previous start time command. + * (Note: This usually applies to second and forward commands from the start time commands map) + * ** 1.2 If yes, create new SNMP commands using the end time. + * ** Iterate the nested map of command type and phases, perform the bitwise or operation on the values using the phases.Create SNMP command for each command type and phases, and add the SNMP command to the queue. + * + * 2. Iterate the nested map of command type and phases. At each start time, multiple command types are expected. Each command type has multiple phases. + * It needs to create streets SNMP command for each command type. The boolean indicators (is_forceoff, is_hold, is_call_veh, is_call_ped, is_omit_veh, is_omit_ped) are used to indicate + * which types of commands should be executed at the current start time. The set_val_hold, set_val_forceoff, set_val_call_veh, set_val_call_ped, set_val_omit_ped, set_val_omit_veh + * values are used to accumulate the phases associated to each command type at the current start time. + * Once finished interating the nested map of command types and phases. It will create the streets SNMP command based on the boolean indicator to corresponding create SNMP command and values for each command type. + * 3. Iterate the end time commands map in case some commands ends the same time as the start time commands. + * 4. Check the boolean indicators and create corresponding SNMP commands. Add the SNMP commands to the queue. + * + * 5. Iterate the end time commands map in case some commands ends after finishing all commands at the start time. + * ** Checking the snmp_cmd_start_time and the end time from the end time command maps. If there are any commands end time that is greater than snmp_cmd_start_time, create streets + * ** SNMP commands for that end time command, and add the SNMP commands to the queue. (Note: At the end of the step 1 iteration, the snmp_cmd_start_time is updated with the maximum start time.) + * */ + //Step 1: Loop through start time commands map + for (auto start_time_cmd_itr = start_time_cmd_m.begin(); start_time_cmd_itr != start_time_cmd_m.end(); start_time_cmd_itr++) + { + // Indicator to note which phase control types are received at a the start time + bool is_hold = false; + bool is_forceoff = false; + bool is_call_veh = false; + bool is_call_ped = false; + bool is_omit_veh = false; + bool is_omit_ped = false; + snmp_cmd_start_time = start_time_cmd_itr->first; + + // Step 1.1 & 1.2: Checking end time for commands and make sure to reset the phases if end time is earlier than start time and is older then previous start time + for (auto end_time_cmd_itr = end_time_cmd_m.begin(); end_time_cmd_itr != end_time_cmd_m.end(); end_time_cmd_itr++) + { + // Command end time is earlier than current command start time and the end time is older than previous start time + if (end_time_cmd_itr->first < start_time_cmd_itr->first && end_time_cmd_itr->first > prev_snmp_start_time) + { + // Loop through each control type and find all phases belong to the control type. Bitwise xor operation on the phases for the end time phases and previous start time phases + for (auto inner_itr = end_time_cmd_itr->second.begin(); inner_itr != end_time_cmd_itr->second.end(); inner_itr++) + { + // Phases at the end time + auto phases = inner_itr->second; + auto phase_control_type = to_phase_control_type(inner_itr->first); + if (phase_control_type == PHASE_CONTROL_TYPE::CALL_PED_PHASES) + { + set_val_call_ped = bitwise_xor_phases(set_val_call_ped, phases); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::CALL_PED_PHASES, static_cast(set_val_call_ped)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::CALL_VEH_PHASES) + { + set_val_call_veh = bitwise_xor_phases(set_val_call_veh, phases); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::CALL_VEH_PHASES, static_cast(set_val_call_veh)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_PED_PHASES) + { + set_val_omit_ped = bitwise_xor_phases(set_val_omit_ped, phases); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::OMIT_PED_PHASES, static_cast(set_val_omit_ped)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + set_val_omit_veh = bitwise_xor_phases(set_val_omit_veh, phases); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, static_cast(set_val_omit_veh)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::FORCEOFF_PHASES) + { + set_val_forceoff = bitwise_xor_phases(set_val_forceoff, phases); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::FORCEOFF_PHASES, static_cast(set_val_forceoff)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + set_val_hold = bitwise_xor_phases(set_val_hold, phases); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, static_cast(set_val_hold)); + } + } + } + } + + // Step 2: Check start time commands, loop through each control type to find the phases. Bitwise or operation on the phases for the current start time and current control type + for (auto inner_itr = start_time_cmd_itr->second.begin(); inner_itr != start_time_cmd_itr->second.end(); inner_itr++) + { + auto phases = inner_itr->second; + auto phase_control_type = to_phase_control_type(inner_itr->first); + if (phase_control_type == PHASE_CONTROL_TYPE::CALL_PED_PHASES) + { + set_val_call_ped = bitwise_or_phases(set_val_call_ped, phases); + is_call_ped = true; + } + else if (phase_control_type == PHASE_CONTROL_TYPE::CALL_VEH_PHASES) + { + set_val_call_veh = bitwise_or_phases(set_val_call_veh, phases); + is_call_veh = true; + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_PED_PHASES) + { + set_val_omit_ped = bitwise_or_phases(set_val_omit_ped, phases); + is_omit_ped = true; + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + set_val_omit_veh = bitwise_or_phases(set_val_omit_veh, phases); + is_omit_veh = true; + } + else if (phase_control_type == PHASE_CONTROL_TYPE::FORCEOFF_PHASES) + { + set_val_forceoff = bitwise_or_phases(set_val_forceoff, phases); + is_forceoff = true; + } + else if (phase_control_type == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + set_val_hold = bitwise_or_phases(set_val_hold, phases); + is_hold = true; + } + } + + // Step 3: Checking end time for commands and make sure to reset the phases if end time equals to start time + for (auto end_time_cmd_itr = end_time_cmd_m.begin(); end_time_cmd_itr != end_time_cmd_m.end(); end_time_cmd_itr++) + { + // Command end time equals to current command start time + if (end_time_cmd_itr->first == start_time_cmd_itr->first) + { + for (auto inner_itr = end_time_cmd_itr->second.begin(); inner_itr != end_time_cmd_itr->second.end(); inner_itr++) + { + auto phase_control_type = to_phase_control_type(inner_itr->first); + if (phase_control_type == PHASE_CONTROL_TYPE::CALL_PED_PHASES) + { + set_val_call_ped = bitwise_xor_phases(set_val_call_ped, inner_itr->second); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::CALL_VEH_PHASES) + { + set_val_call_veh = bitwise_xor_phases(set_val_call_veh, inner_itr->second); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_PED_PHASES) + { + set_val_omit_ped = bitwise_xor_phases(set_val_omit_ped, inner_itr->second); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + set_val_omit_veh = bitwise_xor_phases(set_val_omit_veh, inner_itr->second); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::FORCEOFF_PHASES) + { + set_val_forceoff = bitwise_xor_phases(set_val_forceoff, inner_itr->second); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + set_val_hold = bitwise_xor_phases(set_val_hold, inner_itr->second); + } + } + } + } + + //Step 4: + if(is_hold) + { + push_snmp_command_to_queue(cmds_result, snmp_cmd_start_time, PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, static_cast(set_val_hold)); + } + if(is_forceoff) + { + push_snmp_command_to_queue(cmds_result, snmp_cmd_start_time, PHASE_CONTROL_TYPE::FORCEOFF_PHASES, static_cast(set_val_forceoff)); + } + if(is_omit_veh) + { + push_snmp_command_to_queue(cmds_result, snmp_cmd_start_time, PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, static_cast(set_val_omit_veh)); + } + if(is_omit_ped) + { + push_snmp_command_to_queue(cmds_result, snmp_cmd_start_time, PHASE_CONTROL_TYPE::OMIT_PED_PHASES, static_cast(set_val_omit_ped)); + } + if(is_call_ped) + { + push_snmp_command_to_queue(cmds_result, snmp_cmd_start_time, PHASE_CONTROL_TYPE::CALL_PED_PHASES, static_cast(set_val_call_ped)); + } + if(is_call_veh) + { + push_snmp_command_to_queue(cmds_result, snmp_cmd_start_time, PHASE_CONTROL_TYPE::CALL_VEH_PHASES, static_cast(set_val_call_veh)); + } + // Update previous start time with current start time + prev_snmp_start_time = snmp_cmd_start_time; + } // END Start Time commands + + // Step 5: Checking end time for commands and make sure to reset the phases if end time greater than the maximum (last) start time + for (auto end_time_cmd_itr = end_time_cmd_m.begin(); end_time_cmd_itr != end_time_cmd_m.end(); end_time_cmd_itr++) + { + if (end_time_cmd_itr->first > snmp_cmd_start_time) + { + // Check end time commands, loop through each control type to find the phases. Bitwise xor operation on the phases for the end time (which is greater than maximum start time) and current control type + for (auto inner_itr = end_time_cmd_itr->second.begin(); inner_itr != end_time_cmd_itr->second.end(); inner_itr++) + { + auto phase_control_type = to_phase_control_type(inner_itr->first); + if (phase_control_type == PHASE_CONTROL_TYPE::CALL_PED_PHASES) + { + set_val_call_ped = bitwise_xor_phases(set_val_call_ped, inner_itr->second); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::CALL_PED_PHASES, static_cast(set_val_call_ped)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::CALL_VEH_PHASES) + { + set_val_call_veh = bitwise_xor_phases(set_val_call_veh, inner_itr->second); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::CALL_VEH_PHASES, static_cast(set_val_call_veh)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_PED_PHASES) + { + set_val_omit_ped = bitwise_xor_phases(set_val_omit_ped, inner_itr->second); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::OMIT_PED_PHASES, static_cast(set_val_omit_ped)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + set_val_omit_veh = bitwise_xor_phases(set_val_omit_veh, inner_itr->second); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, static_cast(set_val_omit_veh)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::FORCEOFF_PHASES) + { + set_val_forceoff = bitwise_xor_phases(set_val_forceoff, inner_itr->second); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::FORCEOFF_PHASES, static_cast(set_val_forceoff)); + } + else if (phase_control_type == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + set_val_hold = bitwise_xor_phases(set_val_hold, inner_itr->second); + push_snmp_command_to_queue(cmds_result, end_time_cmd_itr->first, PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, static_cast(set_val_hold)); + } + } + } + }//END end time commands map + + return cmds_result; + } + + uint8_t streets_snmp_cmd_converter::bitwise_or_phases(uint8_t val, const std::vector &phases) const + { + for (auto phase : phases) + { + val |= (1 << (phase - 1)); + } + return val; + } + + uint8_t streets_snmp_cmd_converter::bitwise_xor_phases(uint8_t val, const std::vector &phases) const + { + for (auto phase : phases) + { + val ^= (1 << (phase - 1)); + } + return val; + } + + void streets_snmp_cmd_converter::add_phase_control_schedule_command_to_two_dimension_map(uint64_t start_time, streets_phase_control_schedule::streets_phase_control_command pcs_cmd, std::map>> &time_cmd_m) const + { + // Checking start time, if the start time as key does not exist in the map, add the start time, the phase and control type as new record in the map. + if (time_cmd_m.find(start_time) == time_cmd_m.end()) + { + std::map> cmd_type_phase_m; + std::vector phases; + phases.push_back(pcs_cmd.command_phase); + cmd_type_phase_m.try_emplace(pcs_cmd.command_type, phases); + time_cmd_m.try_emplace(start_time, cmd_type_phase_m); + } + else + { + /*** + * Checking start time, if the start time already exist, then check the command type. + * If it is a new command type, add the phase abd control type as new record in the nest map. + * If the command type already exists, add the phase to the existing phases list. + * ***/ + auto tmp_cmd_phase_m = time_cmd_m.find(start_time)->second; + // Different command types at the same start time + if (tmp_cmd_phase_m.find(pcs_cmd.command_type) == tmp_cmd_phase_m.end()) + { + std::map> cmd_type_phase_m; + std::vector phases; + phases.push_back(pcs_cmd.command_phase); + time_cmd_m.find(start_time)->second.try_emplace(pcs_cmd.command_type, phases); + } + else + { + // Same command type at the same start time + time_cmd_m.find(start_time)->second.find(pcs_cmd.command_type)->second.push_back(pcs_cmd.command_phase); + } + } + } + + void streets_snmp_cmd_converter::print_two_dimension_map(std::map>> &time_cmd_m, bool is_cmd_start) const + { + SPDLOG_DEBUG("---------------Printing Two Dimension Command Map of Phase Control Schedule Command and Phases---------"); + for (auto &cmd : time_cmd_m) + { + // Start/End time + is_cmd_start ? SPDLOG_DEBUG("Start Timestamp: {0}", cmd.first) : SPDLOG_DEBUG("End Timestamp: {0}", cmd.first); + for (auto &cmd_inner : cmd.second) + { + // Command type + streets_phase_control_schedule::streets_phase_control_command cmd_tmp; + std::string command_type_str = cmd_tmp.COMMAND_TYPE_to_string(cmd_inner.first); + SPDLOG_DEBUG("\tCommand Type: {0}", command_type_str); + // Phases + std::string cmd_inner_inner_str = ""; + for (auto &cmd_inner_inner : cmd_inner.second) + { + cmd_inner_inner_str += std::to_string(cmd_inner_inner) + ","; + } + SPDLOG_DEBUG("\t\tPhases: {0}", cmd_inner_inner_str); + } + SPDLOG_DEBUG("\n"); + } + } + + PHASE_CONTROL_TYPE streets_snmp_cmd_converter::to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE command_type) const + { + PHASE_CONTROL_TYPE result; + switch (command_type) + { + case streets_phase_control_schedule::COMMAND_TYPE::CALL_PED_PHASES: + result = PHASE_CONTROL_TYPE::CALL_PED_PHASES; + break; + case streets_phase_control_schedule::COMMAND_TYPE::CALL_VEH_PHASES: + result = PHASE_CONTROL_TYPE::CALL_VEH_PHASES; + break; + case streets_phase_control_schedule::COMMAND_TYPE::FORCEOFF_PHASES: + result = PHASE_CONTROL_TYPE::FORCEOFF_PHASES; + break; + case streets_phase_control_schedule::COMMAND_TYPE::HOLD_VEH_PHASES: + result = PHASE_CONTROL_TYPE::HOLD_VEH_PHASES; + break; + case streets_phase_control_schedule::COMMAND_TYPE::OMIT_PED_PHASES: + result = PHASE_CONTROL_TYPE::OMIT_PED_PHASES; + break; + case streets_phase_control_schedule::COMMAND_TYPE::OMIT_VEH_PHASES: + result = PHASE_CONTROL_TYPE::OMIT_VEH_PHASES; + break; + default: + throw streets_snmp_cmd_exception("Phase control schedule command type does not have a mapping SNMP phase control type."); + } + return result; + } + + std::vector streets_snmp_cmd_converter::create_clear_all_snmp_commands(uint64_t execution_time) const + { + std::vector snmp_cmds_result; + snmp_cmds_result.push_back(create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::CALL_PED_PHASES, execution_time)); + snmp_cmds_result.push_back(create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::CALL_VEH_PHASES, execution_time)); + snmp_cmds_result.push_back(create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_PED_PHASES, execution_time)); + snmp_cmds_result.push_back(create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, execution_time)); + snmp_cmds_result.push_back(create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::FORCEOFF_PHASES, execution_time)); + snmp_cmds_result.push_back(create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, execution_time)); + return snmp_cmds_result; + } + + void streets_snmp_cmd_converter::push_snmp_command_to_queue(std::queue &cmds_queue, uint64_t start_time, PHASE_CONTROL_TYPE command_type, int64_t val) const + { + snmp_cmd_struct command(start_time, command_type, val); + cmds_queue.push(command); + } +} // namespace streets_snmp_cmd diff --git a/streets_utils/streets_snmp_cmd/test/streets_snmp_cmd_converter_test.cpp b/streets_utils/streets_snmp_cmd/test/streets_snmp_cmd_converter_test.cpp new file mode 100644 index 000000000..62458f5e0 --- /dev/null +++ b/streets_utils/streets_snmp_cmd/test/streets_snmp_cmd_converter_test.cpp @@ -0,0 +1,507 @@ + +#include "streets_snmp_cmd_converter.h" +#include "streets_snmp_cmd_exception.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace streets_snmp_cmd; + +namespace +{ + class streets_snmp_cmd_converter_test : public ::testing::Test + { + public: + streets_snmp_cmd_converter converter; + }; + + TEST_F(streets_snmp_cmd_converter_test, create_snmp_command_by_phases) + { + std::vector phases; + phases.push_back(7); + uint64_t start_time = 1686751639; + PHASE_CONTROL_TYPE control_type; + auto command1 = converter.create_snmp_command_by_phases(phases, control_type, start_time); + std::string expected_str = "control_cmd_type:; execution_start_time:1686751639; value_set:64"; + ASSERT_EQ(expected_str, command1.get_cmd_info()); + + phases.push_back(1); + auto command2 = converter.create_snmp_command_by_phases(phases, control_type, start_time); + expected_str = "control_cmd_type:; execution_start_time:1686751639; value_set:65"; + ASSERT_EQ(expected_str, command2.get_cmd_info()); + + phases.push_back(3); + auto command3 = converter.create_snmp_command_by_phases(phases, control_type, start_time); + expected_str = "control_cmd_type:; execution_start_time:1686751639; value_set:69"; + ASSERT_EQ(expected_str, command3.get_cmd_info()); + + phases.push_back(8); + auto command4 = converter.create_snmp_command_by_phases(phases, control_type, start_time); + expected_str = "control_cmd_type:; execution_start_time:1686751639; value_set:197"; + ASSERT_EQ(expected_str, command4.get_cmd_info()); + }; + + TEST_F(streets_snmp_cmd_converter_test, create_snmp_reset_command) + { + uint64_t start_time = 1686751639; + PHASE_CONTROL_TYPE control_type; + auto command = converter.create_snmp_reset_command(control_type, start_time); + std::string expected_str = "control_cmd_type:; execution_start_time:1686751639; value_set:0"; + ASSERT_EQ(expected_str, command.get_cmd_info()); + }; + + TEST_F(streets_snmp_cmd_converter_test, create_snmp_cmds_by_phase_control_schedule) + { + // Different commandType, same phase and start/end time the same + auto pcs = std::make_shared(); + streets_phase_control_schedule::streets_phase_control_command psc_cmd0("hold", 2, 0, 0); + streets_phase_control_schedule::streets_phase_control_command psc_cmd01("omit_ped", 2, 0, 0); + pcs->commands.push_back(psc_cmd0); + pcs->commands.push_back(psc_cmd01); + + // Same commandType, two different phases and different start time, but end time is the same + streets_phase_control_schedule::streets_phase_control_command psc_cmd2("hold", 4, 6, 24); + streets_phase_control_schedule::streets_phase_control_command psc_cmd3("hold", 2, 5, 24); + pcs->commands.push_back(psc_cmd2); + pcs->commands.push_back(psc_cmd3); + + // Same commandType, two different phases and same start time, and end time + streets_phase_control_schedule::streets_phase_control_command psc_cmd4("hold", 4, 25, 28); + streets_phase_control_schedule::streets_phase_control_command psc_cmd5("hold", 2, 25, 28); + pcs->commands.push_back(psc_cmd4); + pcs->commands.push_back(psc_cmd5); + + // Same commandType, two different phases and different start/end time + streets_phase_control_schedule::streets_phase_control_command psc_cmd6("hold", 2, 48, 65); + streets_phase_control_schedule::streets_phase_control_command psc_cmd7("hold", 4, 49, 64); + pcs->commands.push_back(psc_cmd6); + pcs->commands.push_back(psc_cmd7); + + streets_phase_control_schedule::streets_phase_control_command psc_cmd8("forceoff", 4, 49, 69); + pcs->commands.push_back(psc_cmd8); + + // Same commandType, two difference phases and start/end time are the same. + streets_phase_control_schedule::streets_phase_control_command psc_cmd9("omit_veh", 5, 0, 135); + streets_phase_control_schedule::streets_phase_control_command psc_cmd10("omit_veh", 8, 0, 135); + pcs->commands.push_back(psc_cmd9); + pcs->commands.push_back(psc_cmd10); + + // Different commandType and phases, start and end time are the same + streets_phase_control_schedule::streets_phase_control_command psc_cmd13("call_ped", 8, 158, 159); + streets_phase_control_schedule::streets_phase_control_command psc_cmd14("call_veh", 5, 158, 159); + pcs->commands.push_back(psc_cmd13); + pcs->commands.push_back(psc_cmd14); + + streets_phase_control_schedule::streets_phase_control_command psc_cmd15("hold", 1, 160, 200); + streets_phase_control_schedule::streets_phase_control_command psc_cmd16("hold", 2, 161, 199); + streets_phase_control_schedule::streets_phase_control_command psc_cmd17("hold", 3, 162, 198); + streets_phase_control_schedule::streets_phase_control_command psc_cmd18("hold", 4, 163, 197); + streets_phase_control_schedule::streets_phase_control_command psc_cmd19("hold", 5, 164, 196); + streets_phase_control_schedule::streets_phase_control_command psc_cmd20("hold", 6, 165, 195); + streets_phase_control_schedule::streets_phase_control_command psc_cmd21("hold", 7, 166, 194); + streets_phase_control_schedule::streets_phase_control_command psc_cmd22("hold", 8, 167, 193); + pcs->commands.push_back(psc_cmd15); + pcs->commands.push_back(psc_cmd16); + pcs->commands.push_back(psc_cmd17); + pcs->commands.push_back(psc_cmd18); + pcs->commands.push_back(psc_cmd19); + pcs->commands.push_back(psc_cmd20); + pcs->commands.push_back(psc_cmd21); + + auto cmds_result = converter.create_snmp_cmds_by_phase_control_schedule(pcs); + std::string expected_str = ""; + while (!cmds_result.empty()) + { + auto snmp_cmd = cmds_result.front(); + cmds_result.pop(); + SPDLOG_INFO("{0}", snmp_cmd.get_cmd_info()); + if (snmp_cmd.start_time_ == 0) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:0; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + else if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Omit; execution_start_time:0; value_set:144"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + else if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::OMIT_PED_PHASES) + { + expected_str = "control_cmd_type:Pedestrian Omit; execution_start_time:0; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 5) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:5; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 6) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:6; value_set:10"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 24) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:24; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 25) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:25; value_set:10"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 28) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:28; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 48) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:48; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 49) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:49; value_set:10"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + else if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::FORCEOFF_PHASES) + { + expected_str = "control_cmd_type:Vehicle Forceoff; execution_start_time:49; value_set:8"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 64) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:64; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 65) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:65; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 69) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::FORCEOFF_PHASES) + { + expected_str = "control_cmd_type:Vehicle Forceoff; execution_start_time:69; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 135) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Omit; execution_start_time:135; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 158) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::CALL_PED_PHASES) + { + expected_str = "control_cmd_type:Pedestrian Call; execution_start_time:158; value_set:128"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + else if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::CALL_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Call; execution_start_time:158; value_set:16"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 159) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::CALL_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Call; execution_start_time:159; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + else if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Omit; execution_start_time:159; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 160) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:160; value_set:1"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 161) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:161; value_set:3"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 162) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:162; value_set:7"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 163) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:163; value_set:15"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 164) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:164; value_set:31"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 165) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:165; value_set:63"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 166) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:166; value_set:127"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 194) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:194; value_set:63"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 195) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:195; value_set:31"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 196) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:196; value_set:15"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 197) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:197; value_set:7"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 198) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:198; value_set:3"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 199) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:199; value_set:1"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + else if (snmp_cmd.start_time_ == 200) + { + if (snmp_cmd.control_type_ == PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) + { + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:200; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + } + } + } + } + + TEST_F(streets_snmp_cmd_converter_test, create_snmp_cmds_by_phase_control_schedule_2) + { + // Different commandType, different phases, same start time and different end time + auto pcs = std::make_shared(); + streets_phase_control_schedule::streets_phase_control_command psc_cmd0("hold", 1, 0, 10); + streets_phase_control_schedule::streets_phase_control_command psc_cmd1("omit_ped", 2, 0, 12); + streets_phase_control_schedule::streets_phase_control_command psc_cmd2("omit_veh", 3, 0, 13); + streets_phase_control_schedule::streets_phase_control_command psc_cmd3("call_veh", 4, 0, 14); + streets_phase_control_schedule::streets_phase_control_command psc_cmd4("call_ped", 5, 0, 15); + streets_phase_control_schedule::streets_phase_control_command psc_cmd5("forceoff", 6, 0, 16); + pcs->commands.push_back(psc_cmd0); + pcs->commands.push_back(psc_cmd1); + pcs->commands.push_back(psc_cmd2); + pcs->commands.push_back(psc_cmd3); + pcs->commands.push_back(psc_cmd4); + pcs->commands.push_back(psc_cmd5); + auto cmds_result = converter.create_snmp_cmds_by_phase_control_schedule(pcs); + std::string expected_str = ""; + while (!cmds_result.empty()) + { + auto snmp_cmd = cmds_result.front(); + cmds_result.pop(); + SPDLOG_INFO("{0}", snmp_cmd.get_cmd_info()); + if (snmp_cmd.start_time_ == 0) + { + switch (snmp_cmd.control_type_) + { + case PHASE_CONTROL_TYPE::HOLD_VEH_PHASES: + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:0; value_set:1"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + case PHASE_CONTROL_TYPE::FORCEOFF_PHASES: + expected_str = "control_cmd_type:Vehicle Forceoff; execution_start_time:0; value_set:32"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + case PHASE_CONTROL_TYPE::OMIT_VEH_PHASES: + expected_str = "control_cmd_type:Vehicle Omit; execution_start_time:0; value_set:4"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + case PHASE_CONTROL_TYPE::OMIT_PED_PHASES: + expected_str = "control_cmd_type:Pedestrian Omit; execution_start_time:0; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + case PHASE_CONTROL_TYPE::CALL_PED_PHASES: + expected_str = "control_cmd_type:Pedestrian Call; execution_start_time:0; value_set:16"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + case PHASE_CONTROL_TYPE::CALL_VEH_PHASES: + expected_str = "control_cmd_type:Vehicle Call; execution_start_time:0; value_set:8"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + } + } + else if (snmp_cmd.start_time_ == 10) + { + switch (snmp_cmd.control_type_) + { + case PHASE_CONTROL_TYPE::HOLD_VEH_PHASES: + expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:10; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + } + } + else if (snmp_cmd.start_time_ == 12) + { + switch (snmp_cmd.control_type_) + { + case PHASE_CONTROL_TYPE::OMIT_PED_PHASES: + expected_str = "control_cmd_type:Pedestrian Omit; execution_start_time:12; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + } + } + else if (snmp_cmd.start_time_ == 13) + { + switch (snmp_cmd.control_type_) + { + case PHASE_CONTROL_TYPE::OMIT_VEH_PHASES: + expected_str = "control_cmd_type:Vehicle Omit; execution_start_time:13; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + } + } + else if (snmp_cmd.start_time_ == 14) + { + switch (snmp_cmd.control_type_) + { + case PHASE_CONTROL_TYPE::CALL_VEH_PHASES: + expected_str = "control_cmd_type:Vehicle Call; execution_start_time:14; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + } + } + else if (snmp_cmd.start_time_ == 15) + { + switch (snmp_cmd.control_type_) + { + case PHASE_CONTROL_TYPE::CALL_PED_PHASES: + expected_str = "control_cmd_type:Pedestrian Call; execution_start_time:15; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + } + } + else if (snmp_cmd.start_time_ == 16) + { + switch (snmp_cmd.control_type_) + { + case PHASE_CONTROL_TYPE::FORCEOFF_PHASES: + expected_str = "control_cmd_type:Vehicle Forceoff; execution_start_time:16; value_set:0"; + ASSERT_EQ(expected_str, snmp_cmd.get_cmd_info()); + break; + } + } + } + } + + TEST_F(streets_snmp_cmd_converter_test, to_phase_control_type) + { + ASSERT_EQ(PHASE_CONTROL_TYPE::CALL_PED_PHASES, converter.to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE::CALL_PED_PHASES)); + ASSERT_EQ(PHASE_CONTROL_TYPE::CALL_VEH_PHASES, converter.to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE::CALL_VEH_PHASES)); + ASSERT_EQ(PHASE_CONTROL_TYPE::OMIT_PED_PHASES, converter.to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE::OMIT_PED_PHASES)); + ASSERT_EQ(PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, converter.to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE::OMIT_VEH_PHASES)); + ASSERT_EQ(PHASE_CONTROL_TYPE::FORCEOFF_PHASES, converter.to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE::FORCEOFF_PHASES)); + ASSERT_EQ(PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, converter.to_phase_control_type(streets_phase_control_schedule::COMMAND_TYPE::HOLD_VEH_PHASES)); + } + + TEST_F(streets_snmp_cmd_converter_test, create_clear_all_snmp_commands) + { + ASSERT_EQ(6, converter.create_clear_all_snmp_commands(0).size()); + } +}; diff --git a/streets_utils/streets_snmp_cmd/test/streets_snmp_cmd_test.cpp b/streets_utils/streets_snmp_cmd/test/streets_snmp_cmd_test.cpp new file mode 100644 index 000000000..238214646 --- /dev/null +++ b/streets_utils/streets_snmp_cmd/test/streets_snmp_cmd_test.cpp @@ -0,0 +1,58 @@ + +#include "streets_snmp_cmd.h" +#include "streets_snmp_cmd_exception.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace streets_snmp_cmd; + +namespace +{ + + class streets_snmp_cmd_test : public ::testing::Test + { + }; + + TEST_F(streets_snmp_cmd_test, get_cmd_info) + { + uint64_t start_time = 1686683893; + PHASE_CONTROL_TYPE type = PHASE_CONTROL_TYPE::HOLD_VEH_PHASES; + int64_t val = 2; + snmp_cmd_struct snmp_cmd1(start_time, type, val); + std::string expected_str = "control_cmd_type:Vehicle Hold; execution_start_time:1686683893; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd1.get_cmd_info()); + + type = PHASE_CONTROL_TYPE::CALL_PED_PHASES; + snmp_cmd_struct snmp_cmd2(start_time, type, val); + expected_str = "control_cmd_type:Pedestrian Call; execution_start_time:1686683893; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd2.get_cmd_info()); + + type = PHASE_CONTROL_TYPE::CALL_VEH_PHASES; + snmp_cmd_struct snmp_cmd3(start_time, type, val); + expected_str = "control_cmd_type:Vehicle Call; execution_start_time:1686683893; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd3.get_cmd_info()); + + type = PHASE_CONTROL_TYPE::OMIT_PED_PHASES; + snmp_cmd_struct snmp_cmd4(start_time, type, val); + expected_str = "control_cmd_type:Pedestrian Omit; execution_start_time:1686683893; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd4.get_cmd_info()); + + type = PHASE_CONTROL_TYPE::OMIT_VEH_PHASES; + snmp_cmd_struct snmp_cmd5(start_time, type, val); + expected_str = "control_cmd_type:Vehicle Omit; execution_start_time:1686683893; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd5.get_cmd_info()); + + type = PHASE_CONTROL_TYPE::FORCEOFF_PHASES; + snmp_cmd_struct snmp_cmd6(start_time, type, val); + expected_str = "control_cmd_type:Vehicle Forceoff; execution_start_time:1686683893; value_set:2"; + ASSERT_EQ(expected_str, snmp_cmd6.get_cmd_info()); + } +}; \ No newline at end of file diff --git a/streets_utils/streets_snmp_cmd/test/test_main.cpp b/streets_utils/streets_snmp_cmd/test/test_main.cpp new file mode 100644 index 000000000..1e925a90b --- /dev/null +++ b/streets_utils/streets_snmp_cmd/test/test_main.cpp @@ -0,0 +1,6 @@ +#include "gtest/gtest.h" +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/streets_utils/streets_timing_plan/CMakeLists.txt b/streets_utils/streets_timing_plan/CMakeLists.txt new file mode 100644 index 000000000..d8c60ab02 --- /dev/null +++ b/streets_utils/streets_timing_plan/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.10.2) +project(streets_timing_plan) +######################################################## +# Find Dependent Packages and set configurations +######################################################## +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +find_package(spdlog REQUIRED) +find_package(GTest REQUIRED CONFIG) +add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) +find_package(RapidJSON REQUIRED) +# Add definition for rapidjson to include std::string +add_definitions(-DRAPIDJSON_HAS_STDSTRING=1) + +######################################################## +# Build Library +######################################################## +file(GLOB SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/**/*.cpp) +add_library(${PROJECT_NAME}_lib ${SOURCES} ) +target_include_directories(${PROJECT_NAME}_lib + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) +target_link_libraries( ${PROJECT_NAME}_lib + PUBLIC + spdlog::spdlog + rapidjson +) + +######################################################## +# Install streets_timing_plan package. +######################################################## +file(GLOB files ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h) +file(GLOB templates ${CMAKE_CURRENT_SOURCE_DIR}/include/internal/*.tpp) +install( + TARGETS ${PROJECT_NAME}_lib + EXPORT ${PROJECT_NAME}_libTargets + LIBRARY DESTINATION lib + INCLUDES DESTINATION include + ARCHIVE DESTINATION lib +) +install( + EXPORT ${PROJECT_NAME}_libTargets + FILE ${PROJECT_NAME}_libTargets.cmake + DESTINATION lib/cmake/${PROJECT_NAME}_lib/ + NAMESPACE ${PROJECT_NAME}_lib:: +) +include(CMakePackageConfigHelpers) +configure_package_config_file( + cmake/${PROJECT_NAME}_libConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_libConfig.cmake + INSTALL_DESTINATION lib/${PROJECT_NAME}_lib/${PROJECT_NAME}_lib/ ) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_libConfig.cmake + DESTINATION lib/cmake/${PROJECT_NAME}_lib/ +) +install(FILES ${files} DESTINATION include) + +######################## +# googletest for unit testing +######################## +set(BINARY ${PROJECT_NAME}_test) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) +set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +add_executable(${BINARY} ${TEST_SOURCES}) +add_test(NAME ${BINARY} COMMAND ${BINARY}) +target_include_directories(${BINARY} PUBLIC ${PROJECT_SOURCE_DIR}/include) +target_link_libraries(${BINARY} + PUBLIC + ${PROJECT_NAME}_lib +) + +if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.20") + target_link_libraries( ${BINARY} PUBLIC GTest::gtest ) +else() + target_link_libraries( ${BINARY} PUBLIC gtest ) +endif() \ No newline at end of file diff --git a/streets_utils/streets_timing_plan/README.md b/streets_utils/streets_timing_plan/README.md new file mode 100644 index 000000000..a3f62251b --- /dev/null +++ b/streets_utils/streets_timing_plan/README.md @@ -0,0 +1,110 @@ +# Streets Timing Plan Command library +## Introduction +This CARMA-streets library is meant to handle JSON deserialization and serialization for external signal timing plan message generated by [MMITSS Roadside Processor] (https://github.com/mmitss/mmitss-az), and a sample signal timing plan message refers to https://github.com/mmitss/mmitss-az/blob/master/src/mrp/priority-request-solver/test/tci-msg-sender/signalPlan.json. + +### Sample +A sample timing plan: +``` +{ + "MsgType": "ActiveTimingPlan", + "TimingPlan": { + "NoOfPhase": 8, + "PhaseNumber": [ + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8 + ], + "PedWalk": [ + 0, + 7, + 0, + 7, + 0, + 7, + 0, + 7 + ], + "PedClear": [ + 0, + 33, + 0, + 43, + 0, + 33, + 0, + 33 + ], + "MinGreen": [ + 4, + 15, + 4, + 15, + 4, + 15, + 4, + 15 + ], + "Passage": [ + 2.0, + 5.0, + 2.0, + 5.0, + 2.0, + 5.0, + 2.0, + 5.0 + ], + "MaxGreen": [ + 37, + 35, + 19, + 40, + 32, + 40, + 19, + 29 + ], + "YellowChange": [ + 3.0, + 4.0, + 3.0, + 3.6, + 3.0, + 4.0, + 3.0, + 3.6 + ], + "RedClear": [ + 1.0, + 2.5, + 1.0, + 3.4000000000000004, + 1.0, + 2.5, + 1.0, + 3.4000000000000004 + ], + "PhaseRing": [ + 1, + 1, + 1, + 1, + 2, + 2, + 2, + 2 + ] + } +} +``` +## Including library +``` +find_package( streets_timing_plan_lib REQUIRED ) +... +target_link_libraries( PUBLIC streets_timing_plan_lib::streets_timing_plan_lib ) +``` diff --git a/streets_utils/streets_timing_plan/cmake/streets_timing_plan_libConfig.cmake.in b/streets_utils/streets_timing_plan/cmake/streets_timing_plan_libConfig.cmake.in new file mode 100644 index 000000000..990da0c79 --- /dev/null +++ b/streets_utils/streets_timing_plan/cmake/streets_timing_plan_libConfig.cmake.in @@ -0,0 +1,7 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(spdlog REQUIRED) +find_dependency(RapidJSON REQUIRED) + +include("${CMAKE_CURRENT_LIST_DIR}/streets_timing_plan_libTargets.cmake") diff --git a/streets_utils/streets_timing_plan/include/streets_timing_plan.h b/streets_utils/streets_timing_plan/include/streets_timing_plan.h new file mode 100755 index 000000000..ac7e178d3 --- /dev/null +++ b/streets_utils/streets_timing_plan/include/streets_timing_plan.h @@ -0,0 +1,42 @@ +#pragma once +#include "streets_timing_plan_exception.h" +#include +#include +#include +#include +#include +#include +#include +#include "streets_phase_control_command.h" +#include + +namespace streets_timing_plan +{ + + struct streets_timing_plan + { + const std::string TIMING_PLAN_MSG_TYPE = "ActiveTimingPlan"; + std::string msg_type; + int number_of_phase = 0; + std::vector phase_number_v; + std::vector pedestrian_walk_v; + std::vector pedestrian_clear_v; + std::vector min_green_v; + std::vector passage_v; + std::vector max_green_v; + std::vector yellow_change_v; + std::vector red_clear_v; + std::vector phase_ring_v; + + /** + * @brief Deserialize Timing Plan JSON into Timing Plan object. + * + * @param valTiming Plan JSON. + */ + void fromJson(const std::string &json); + /** + * @brief Serialize Timing Plan into JSON String + */ + rapidjson::Document toJson() const; + }; +} \ No newline at end of file diff --git a/streets_utils/streets_timing_plan/include/streets_timing_plan_exception.h b/streets_utils/streets_timing_plan/include/streets_timing_plan_exception.h new file mode 100644 index 000000000..60ddddc25 --- /dev/null +++ b/streets_utils/streets_timing_plan/include/streets_timing_plan_exception.h @@ -0,0 +1,23 @@ +#pragma once + +#include + + + +namespace streets_timing_plan { + /** + * @brief Runtime error related to processing streets_timing_plan. + */ + class streets_timing_plan_exception : public std::runtime_error{ + public: + /** + * @brief Destructor. + */ + ~streets_timing_plan_exception() override; + /** + * @brief Constructor. + * @param msg String exception message. + */ + explicit streets_timing_plan_exception(const std::string &msg ); + }; +} \ No newline at end of file diff --git a/streets_utils/streets_timing_plan/src/exceptions/streets_timing_plan_exception.cpp b/streets_utils/streets_timing_plan/src/exceptions/streets_timing_plan_exception.cpp new file mode 100644 index 000000000..7cbbff354 --- /dev/null +++ b/streets_utils/streets_timing_plan/src/exceptions/streets_timing_plan_exception.cpp @@ -0,0 +1,8 @@ +#include "streets_timing_plan_exception.h" + +namespace streets_timing_plan { + + streets_timing_plan_exception::streets_timing_plan_exception(const std::string &msg): std::runtime_error(msg){}; + + streets_timing_plan_exception::~streets_timing_plan_exception() = default; +} \ No newline at end of file diff --git a/streets_utils/streets_timing_plan/src/models/streets_timing_plan.cpp b/streets_utils/streets_timing_plan/src/models/streets_timing_plan.cpp new file mode 100755 index 000000000..8f1840c92 --- /dev/null +++ b/streets_utils/streets_timing_plan/src/models/streets_timing_plan.cpp @@ -0,0 +1,262 @@ +#include "streets_timing_plan.h" +#include + +namespace streets_timing_plan +{ + void streets_timing_plan::fromJson(const std::string &json) + { + rapidjson::Document doc; + doc.Parse(json); + if (doc.HasParseError()) + { + throw streets_timing_plan_exception("streets_timing_plan message JSON is misformatted."); + } + + if (doc.HasMember("MsgType") && doc.FindMember("MsgType")->value.IsString()) + { + std::string value = doc.FindMember("MsgType")->value.GetString(); + if (value != TIMING_PLAN_MSG_TYPE) + { + throw streets_timing_plan_exception("streets_timing_plan requires MsgType property value (= " + TIMING_PLAN_MSG_TYPE + "), but received " + value + " instead."); + } + msg_type = value; + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required MsgType property."); + } + + if (!doc.HasMember("TimingPlan") || !doc.FindMember("TimingPlan")->value.IsObject()) + { + throw streets_timing_plan_exception("streets_timing_plan is missing required TimingPlan property."); + } + + auto timing_plan_value = doc.FindMember("TimingPlan")->value.GetObject(); + if (timing_plan_value.HasMember("NoOfPhase") && timing_plan_value.FindMember("NoOfPhase")->value.IsUint64()) + { + number_of_phase = timing_plan_value.FindMember("NoOfPhase")->value.GetUint(); + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required NoOfPhase property."); + } + + if (timing_plan_value.HasMember("PhaseNumber") && timing_plan_value.FindMember("PhaseNumber")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("PhaseNumber")->value.GetArray()) + { + phase_number_v.push_back(itr.GetInt()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required PhaseNumber property."); + } + + if (timing_plan_value.HasMember("PedWalk") && timing_plan_value.FindMember("PedWalk")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("PedWalk")->value.GetArray()) + { + pedestrian_walk_v.push_back(itr.GetInt()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required PedWalk property."); + } + + if (timing_plan_value.HasMember("PedClear") && timing_plan_value.FindMember("PedClear")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("PedClear")->value.GetArray()) + { + pedestrian_clear_v.push_back(itr.GetInt()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required PedClear property."); + } + + if (timing_plan_value.HasMember("MinGreen") && timing_plan_value.FindMember("MinGreen")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("MinGreen")->value.GetArray()) + { + min_green_v.push_back(itr.GetInt()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required MinGreen property."); + } + + if (timing_plan_value.HasMember("Passage") && timing_plan_value.FindMember("Passage")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("Passage")->value.GetArray()) + { + passage_v.push_back(itr.GetDouble()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required Passage property."); + } + + if (timing_plan_value.HasMember("MaxGreen") && timing_plan_value.FindMember("MaxGreen")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("MaxGreen")->value.GetArray()) + { + max_green_v.push_back(itr.GetInt()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required MaxGreen property."); + } + + if (timing_plan_value.HasMember("YellowChange") && timing_plan_value.FindMember("YellowChange")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("YellowChange")->value.GetArray()) + { + yellow_change_v.push_back(itr.GetDouble()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required YellowChange property."); + } + + if (timing_plan_value.HasMember("RedClear") && timing_plan_value.FindMember("RedClear")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("RedClear")->value.GetArray()) + { + red_clear_v.push_back(itr.GetDouble()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required RedClear property."); + } + + if (timing_plan_value.HasMember("PhaseRing") && timing_plan_value.FindMember("PhaseRing")->value.IsArray()) + { + for (const auto &itr : timing_plan_value.FindMember("PhaseRing")->value.GetArray()) + { + phase_ring_v.push_back(itr.GetInt()); + } + } + else + { + throw streets_timing_plan_exception("streets_timing_plan is missing required PhaseRing property."); + } + } + + rapidjson::Document streets_timing_plan::toJson() const + { + // document is the root of a JSON message + rapidjson::Document document; + + // Define the document as an object + document.SetObject(); + + // Must pass an allocator when the object may need to allocate memory + rapidjson::Document::AllocatorType &allocator = document.GetAllocator(); + + document.AddMember("MsgType", TIMING_PLAN_MSG_TYPE, allocator); + + // Create a rapidjson timing plan object ytpe + rapidjson::Value timing_plan_obj_value(rapidjson::kObjectType); + timing_plan_obj_value.AddMember("NoOfPhase", number_of_phase, allocator); + + // Create a rapidjson array type for number of phases + rapidjson::Value phase_number_array_value(rapidjson::kArrayType); + + // Add an array of phase number to JSON array + for (const auto &phase_num : phase_number_v) + { + phase_number_array_value.PushBack(phase_num, allocator); + } + timing_plan_obj_value.AddMember("PhaseNumber", phase_number_array_value, allocator); + + // Create a rapidjson array type for pedestrian walk + rapidjson::Value pede_walk_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian walk to rapidjson array + for (const auto &ped_walk : pedestrian_walk_v) + { + pede_walk_array_value.PushBack(ped_walk, allocator); + } + timing_plan_obj_value.AddMember("PedWalk", pede_walk_array_value, allocator); + + // Create a rapidjson array type for pedestrian clear + rapidjson::Value pede_clear_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian clear to rapidjson array + for (const auto &pede_clear : pedestrian_clear_v) + { + pede_clear_array_value.PushBack(pede_clear, allocator); + } + timing_plan_obj_value.AddMember("PedClear", pede_clear_array_value, allocator); + + // Create a rapidjson array type for minimum green + rapidjson::Value min_green_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian clear to rapidjson array + for (const auto &min_green : min_green_v) + { + min_green_array_value.PushBack(min_green, allocator); + } + timing_plan_obj_value.AddMember("MinGreen", min_green_array_value, allocator); + + // Create a rapidjson array type for passage + rapidjson::Value passage_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian clear to rapidjson array + for (const auto &passage : passage_v) + { + passage_array_value.PushBack(passage, allocator); + } + timing_plan_obj_value.AddMember("Passage", passage_array_value, allocator); + + // Create a rapidjson array type for maximum green + rapidjson::Value max_green_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian clear to rapidjson array + for (const auto &max_green : max_green_v) + { + max_green_array_value.PushBack(max_green, allocator); + } + timing_plan_obj_value.AddMember("MaxGreen", max_green_array_value, allocator); + + // Create a rapidjson array type for yellow change + rapidjson::Value yellow_change_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian clear to rapidjson array + for (const auto &yellow_change : yellow_change_v) + { + yellow_change_array_value.PushBack(yellow_change, allocator); + } + timing_plan_obj_value.AddMember("YellowChange", yellow_change_array_value, allocator); + + // Create a rapidjson array type for red clear + rapidjson::Value red_clear_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian clear to rapidjson array + for (const auto &red_clear : red_clear_v) + { + red_clear_array_value.PushBack(red_clear, allocator); + } + timing_plan_obj_value.AddMember("RedClear", red_clear_array_value, allocator); + + // Create a rapidjson array type for phase ring + rapidjson::Value phase_ring_array_value(rapidjson::kArrayType); + + // Add an array of pedestrian clear to rapidjson array + for (const auto &phase_ring : phase_ring_v) + { + phase_ring_array_value.PushBack(phase_ring, allocator); + } + timing_plan_obj_value.AddMember("PhaseRing", phase_ring_array_value, allocator); + document.AddMember("TimingPlan", timing_plan_obj_value, allocator); + return document; + } +} \ No newline at end of file diff --git a/streets_utils/streets_timing_plan/test/streets_timing_plan_test.cpp b/streets_utils/streets_timing_plan/test/streets_timing_plan_test.cpp new file mode 100644 index 000000000..f28456377 --- /dev/null +++ b/streets_utils/streets_timing_plan/test/streets_timing_plan_test.cpp @@ -0,0 +1,195 @@ + +#include "streets_timing_plan.h" +#include "streets_timing_plan_exception.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace streets_timing_plan; + +namespace +{ + + class streets_timing_plan_test : public ::testing::Test + { + /** + * @brief Test Setup method run before each test. + */ + void SetUp() override + { + } + /** + * @brief Test TearDown method run after each test. + */ + void TearDown() override + { + } + }; +}; + +TEST_F(streets_timing_plan_test, toJson) +{ + streets_timing_plan::streets_timing_plan timing_plan; + timing_plan.number_of_phase = 8; + std::vector phase_numbers_v{1, 2, 3, 4, 5, 6, 7, 8}; + std::swap(timing_plan.phase_number_v, phase_numbers_v); + std::vector pede_walk_v{0, 7, 0, 7, 0, 7, 0, 7}; + std::swap(timing_plan.pedestrian_walk_v, pede_walk_v); + std::vector pede_clear_v{0, 33, 1, 43, 0, 33, 0, 33}; + std::swap(timing_plan.pedestrian_clear_v, pede_clear_v); + std::vector min_green_v{4, 15, 4, 15, 4, 15, 4, 15}; + std::swap(timing_plan.min_green_v, min_green_v); + std::vector passage_v{2.0, 5.0, 2.0, 5.0, 2.0, 5.0, 2.0, 5.0}; + std::swap(timing_plan.passage_v, passage_v); + std::vector max_green_v{37, 35, 19, 40, 32, 19, 29}; + std::swap(timing_plan.max_green_v, max_green_v); + std::vector yellow_change_v{3.0, 4.0, 3.0, 3.6, 4.0, 3.0, 3.6}; + std::swap(timing_plan.yellow_change_v, yellow_change_v); + std::vector red_clear_v{1.0, 2.5, 1.0, 3.40000000000004, 1.0, 2.5, 1.0, 3.4000000000000000000004}; + std::swap(timing_plan.red_clear_v, red_clear_v); + std::vector phase_ring_v{1, 1, 1, 1, 1, 2, 2, 2, 2}; + std::swap(timing_plan.phase_ring_v, phase_ring_v); + + auto timing_plan_json_value = timing_plan.toJson(); + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + timing_plan_json_value.Accept(writer); + std::string expected_str = "{\"MsgType\":\"ActiveTimingPlan\",\"TimingPlan\":{\"NoOfPhase\":8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33],\"MinGreen\":[4,15,4,15,4,15,4,15],\"Passage\":[2.0,5.0,2.0,5.0,2.0,5.0,2.0,5.0],\"MaxGreen\":[37,35,19,40,32,19,29],\"YellowChange\":[3.0,4.0,3.0,3.6,4.0,3.0,3.6],\"RedClear\":[1.0,2.5,1.0,3.40000000000004,1.0,2.5,1.0,3.4],\"PhaseRing\":[1,1,1,1,1,2,2,2,2]}}"; + ASSERT_EQ(expected_str, strbuf.GetString()); +} + +TEST_F(streets_timing_plan_test, fromJson) +{ + streets_timing_plan::streets_timing_plan timing_plan; + // Valid JSON + std::string expected_str = "{\"MsgType\":\"ActiveTimingPlan\",\"TimingPlan\":{\"NoOfPhase\":8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33],\"MinGreen\":[4,15,4,15,4,15,4,15],\"Passage\":[2.1,5.0,2.0,5.0,2.0,5.0,2.0,5.0],\"MaxGreen\":[37,35,19,40,32,19,29],\"YellowChange\":[3.0,4.0,3.0,3.6,4.0,3.0,3.6],\"RedClear\":[1.0,2.5,1.0,3.40000000000004,1.0,2.5,1.0,3.4],\"PhaseRing\":[1,1,1,1,1,2,2,2,2]}}"; + timing_plan.fromJson(expected_str); + ASSERT_EQ(8, timing_plan.number_of_phase); + ASSERT_EQ(timing_plan.TIMING_PLAN_MSG_TYPE, timing_plan.msg_type); + ASSERT_EQ(8, timing_plan.phase_number_v.size()); + std::stringstream ss; + for (auto phase_num : timing_plan.phase_number_v) + { + ss << phase_num; + } + ASSERT_EQ("12345678", ss.str()); + + ss.str(""); + for (auto ped_walk : timing_plan.pedestrian_walk_v) + { + ss << ped_walk; + } + ASSERT_EQ("07070707", ss.str()); + + ss.str(""); + for (auto ped_clear : timing_plan.pedestrian_clear_v) + { + ss << ped_clear; + } + ASSERT_EQ("033143033033", ss.str()); + + ss.str(""); + for (auto min_green : timing_plan.min_green_v) + { + ss << min_green; + } + ASSERT_EQ("415415415415", ss.str()); + + ss.str(""); + for (auto passage : timing_plan.passage_v) + { + ss << passage << ","; + } + ASSERT_EQ("2.1,5,2,5,2,5,2,5,", ss.str()); + + ss.str(""); + for (auto max_green : timing_plan.max_green_v) + { + ss << max_green; + } + ASSERT_EQ("37351940321929", ss.str()); + + ss.str(""); + for (auto yellow_change : timing_plan.yellow_change_v) + { + ss << yellow_change << ","; + } + ASSERT_EQ("3,4,3,3.6,4,3,3.6,", ss.str()); + + ss.str(""); + for (auto red_clear : timing_plan.red_clear_v) + { + ss << red_clear << ","; + } + ASSERT_EQ("1,2.5,1,3.4,1,2.5,1,3.4,", ss.str()); + + ss.str(""); + for (auto phase_ring : timing_plan.phase_ring_v) + { + ss << phase_ring << ","; + } + ASSERT_EQ("1,1,1,1,1,2,2,2,2,", ss.str()); + + // Malformatted JSON + std::string invalid_json = "{invalid}"; + ASSERT_THROW(timing_plan.fromJson(invalid_json), streets_timing_plan_exception); + + // Missing MsgType + std::string input_json = "{\"key\":\"value\"}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Incorrect MsgType + input_json = "{\"MsgType\":\"Missing\"}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing TimingPlan property + input_json = "{\"MsgType\":\"ActiveTimingPlan\"}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing NoOfPhase property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"invalid\": \"invalid\"}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing PhaseNumber property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing PedWalk property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing PedClear property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing MinGreen property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing Passage property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33],\"MinGreen\":[4,15,4,15,4,15,4,15]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing MaxGreen property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33],\"MinGreen\":[4,15,4,15,4,15,4,15],\"Passage\":[2.0,5.0,2.0,5.0,2.0,5.0,2.0,5.0]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing YellowChange property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33],\"MinGreen\":[4,15,4,15,4,15,4,15],\"Passage\":[2.0,5.0,2.0,5.0,2.0,5.0,2.0,5.0],\"MaxGreen\":[37,35,19,40,32,19,29]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing RedClear property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33],\"MinGreen\":[4,15,4,15,4,15,4,15],\"Passage\":[2.0,5.0,2.0,5.0,2.0,5.0,2.0,5.0],\"MaxGreen\":[37,35,19,40,32,19,29],\"YellowChange\":[3.0,4.0,3.0,3.6,4.0,3.0,3.6]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); + + // Missing PhaseRing property + input_json = "{\"MsgType\":\"ActiveTimingPlan\", \"TimingPlan\": {\"NoOfPhase\": 8,\"PhaseNumber\":[1,2,3,4,5,6,7,8],\"PedWalk\":[0,7,0,7,0,7,0,7],\"PedClear\":[0,33,1,43,0,33,0,33],\"MinGreen\":[4,15,4,15,4,15,4,15],\"Passage\":[2.0,5.0,2.0,5.0,2.0,5.0,2.0,5.0],\"MaxGreen\":[37,35,19,40,32,19,29],\"YellowChange\":[3.0,4.0,3.0,3.6,4.0,3.0,3.6],\"RedClear\":[1.0,2.5,1.0,3.40000000000004,1.0,2.5,1.0,3.4]}}"; + ASSERT_THROW(timing_plan.fromJson(input_json), streets_timing_plan_exception); +} \ No newline at end of file diff --git a/streets_utils/streets_timing_plan/test/test_main.cpp b/streets_utils/streets_timing_plan/test/test_main.cpp new file mode 100644 index 000000000..1e925a90b --- /dev/null +++ b/streets_utils/streets_timing_plan/test/test_main.cpp @@ -0,0 +1,6 @@ +#include "gtest/gtest.h" +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/streets_utils/streets_vehicle_scheduler/CMakeLists.txt b/streets_utils/streets_vehicle_scheduler/CMakeLists.txt index beaef9b18..79292fd91 100644 --- a/streets_utils/streets_vehicle_scheduler/CMakeLists.txt +++ b/streets_utils/streets_vehicle_scheduler/CMakeLists.txt @@ -4,6 +4,9 @@ project(streets_vehicle_scheduler) # Find Dependent Packages and set configurations ######################################################## set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC ") +# std::shared_mutex and std::map::try_emplace +set(CMAKE_CXX_STANDARD 17) + find_package(spdlog REQUIRED) find_package(RapidJSON REQUIRED) add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) diff --git a/streets_utils/streets_vehicle_scheduler/test/test_signalized_scenario.cpp b/streets_utils/streets_vehicle_scheduler/test/test_signalized_scenario.cpp index a663075d9..fef280ff7 100644 --- a/streets_utils/streets_vehicle_scheduler/test/test_signalized_scenario.cpp +++ b/streets_utils/streets_vehicle_scheduler/test/test_signalized_scenario.cpp @@ -31,6 +31,7 @@ namespace { */ void SetUp() override { + streets_service::streets_clock_singleton::create(false); schedule = std::make_shared(); // the schedule timestamp is set to 10000 tenths of seconds from the current hour. uint64_t hour_tenth_secs = 10000; diff --git a/tsc_client_service/CMakeLists.txt b/tsc_client_service/CMakeLists.txt index 211a02eeb..69f13d341 100644 --- a/tsc_client_service/CMakeLists.txt +++ b/tsc_client_service/CMakeLists.txt @@ -13,6 +13,8 @@ find_package(streets_service_base_lib REQUIRED) find_package(streets_signal_phase_and_timing_lib REQUIRED) find_package(streets_tsc_configuration_lib REQUIRED) find_package(streets_desired_phase_plan_lib REQUIRED) +find_package(streets_phase_control_schedule_lib REQUIRED) +find_package(streets_snmp_cmd_lib REQUIRED) find_library(NETSNMPAGENT "netsnmpagent") find_library(NETSNMPMIBS "netsnmpmibs") find_library(NETSNMP "netsnmp") @@ -40,6 +42,7 @@ add_library(${PROJECT_NAME}_lib src/monitor_tsc_state.cpp src/monitor_desired_phase_plan.cpp src/control_tsc_state.cpp + src/spat_projection_mode.cpp src/exceptions/control_tsc_state_exception.cpp) target_link_libraries( ${PROJECT_NAME}_lib PUBLIC @@ -49,6 +52,8 @@ target_link_libraries( ${PROJECT_NAME}_lib streets_signal_phase_and_timing_lib streets_tsc_configuration_lib streets_desired_phase_plan_lib + streets_phase_control_schedule_lib::streets_phase_control_schedule_lib + streets_snmp_cmd_lib::streets_snmp_cmd_lib intersection_client_api_lib spdlog::spdlog Qt5::Core diff --git a/tsc_client_service/Dockerfile b/tsc_client_service/Dockerfile index 8181cea7d..0c7a8de73 100644 --- a/tsc_client_service/Dockerfile +++ b/tsc_client_service/Dockerfile @@ -11,7 +11,7 @@ RUN mkdir -p /home/carma-streets/ # Install librdkafka RUN echo " ------> Install librdkafka..." WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/confluentinc/librdkafka.git +RUN git clone https://github.com/confluentinc/librdkafka.git -b v2.2.0 WORKDIR /home/carma-streets/ext/librdkafka/ RUN cmake -H. -B_cmake_build RUN cmake --build _cmake_build @@ -20,8 +20,9 @@ RUN cmake --build _cmake_build --target install # Install rapidjson RUN echo " ------> Install rapidjson..." WORKDIR /home/carma-streets/ext -RUN git clone https://github.com/Tencent/rapidjson +RUN git clone https://github.com/Tencent/rapidjson WORKDIR /home/carma-streets/ext/rapidjson/ +RUN git checkout a95e013b97ca6523f32da23f5095fcc9dd6067e5 RUN mkdir build WORKDIR /home/carma-streets/ext/rapidjson/build RUN cmake .. && make -j @@ -84,6 +85,15 @@ RUN cmake -DCMAKE_BUILD_TYPE="Debug" .. RUN make RUN make install +# Install streets_phase_control_schedule +RUN echo " ------> Install streets phase control schedule from streets_utils..." +WORKDIR /home/carma-streets/streets_utils/streets_phase_control_schedule +RUN mkdir build +WORKDIR /home/carma-streets/streets_utils/streets_phase_control_schedule/build +RUN cmake -DCMAKE_BUILD_TYPE="Debug" .. +RUN make +RUN make install + # Install streets_signal_phase_and_timing RUN echo " ------> Install streets service base library from streets_utils..." WORKDIR /home/carma-streets/streets_utils/streets_signal_phase_and_timing @@ -93,6 +103,14 @@ RUN cmake -DCMAKE_BUILD_TYPE="Debug" .. RUN make RUN make install +# Install streets_snmp_cmd +RUN echo " ------> Install streets snmp command from streets_utils..." +WORKDIR /home/carma-streets/streets_utils/streets_snmp_cmd +RUN mkdir build +WORKDIR /home/carma-streets/streets_utils/streets_snmp_cmd/build +RUN cmake -DCMAKE_BUILD_TYPE="Debug" .. +RUN make +RUN make install # Install net-snmp WORKDIR /home/carma-streets/ext/ diff --git a/tsc_client_service/README.md b/tsc_client_service/README.md index e64ea1b07..6c41a8630 100644 --- a/tsc_client_service/README.md +++ b/tsc_client_service/README.md @@ -32,6 +32,8 @@ The `intersection_client` is a REST client implemented using the `streets_utils/ The `spat_worker` is a class which encapsulates a UDP socket listener. This socket listener, listens for UDP NTCIP data packets set from the **TSC** at 10 hz that provide traffic signal state information required for populating the **SPaT**. The `spat_worker` contains a method to consume a UDP datapacket and update the `spat` pointer which stores the most up-to-date information of the traffic signal controller state. The `tsc_service` `spat_thread` then continously consumes these messages and publishes the resulting **SPaT** JSON on the CARMA-Streets Kafka broker. - +## tsc_service to MMITSS integration +### Configuration parameter +The `use_mmitss_mrp` parameter in manifest.json file is used to determine whether the TSC service consume [phase control schedule](https://github.com/mmitss/mmitss-az/tree/master/src/mrp/traffic-controller-interface) from external [MMTISS Roadside processor](https://github.com/mmitss/mmitss-az) or not. By default, this parameter is set to false to consume desired phase plan from internal signal optimization service. The MRP and carma-streets signal optimization service shall run exclusively, meaning there is no scenario where both MRP and signal optmization service are trying to manipulate traffic signal controller at the same time. diff --git a/tsc_client_service/include/control_tsc_state.h b/tsc_client_service/include/control_tsc_state.h index 2a7146de3..3e656aa4a 100644 --- a/tsc_client_service/include/control_tsc_state.h +++ b/tsc_client_service/include/control_tsc_state.h @@ -1,6 +1,8 @@ #pragma once #include "streets_desired_phase_plan.h" +#include "streets_phase_control_schedule.h" +#include "streets_snmp_cmd_converter.h" #include "snmp_client.h" #include "ntcip_oids.h" #include "monitor_tsc_state.h" @@ -9,66 +11,6 @@ namespace traffic_signal_controller_service { - - /** @brief Object to store snmp control commands. Contructed with an initialized snmp_client_worker_ this object stores SNMP HOLD and OMIT commands - to be executed at specified time */ - struct snmp_cmd_struct - { - /** @brief The type of control being set on the TSC */ - enum class control_type{ - Hold, - Omit - }; - /* Pointer to a snmp client object which can execute snmp commands*/ - std::shared_ptr snmp_client_worker_; - /*Value to be set for Hold/Omit*/ - snmp_response_obj set_val_; - /*Time at which the snmp set command should be executed*/ - uint64_t start_time_; - /*Type of the snmp set command this object creates- Hold or Omit*/ - control_type control_type_; - - snmp_cmd_struct(std::shared_ptr snmp_client_worker, int64_t start_time, control_type type, int64_t val) - : snmp_client_worker_(snmp_client_worker), start_time_(start_time), control_type_(type) - { - set_val_.type = snmp_response_obj::response_type::INTEGER; - set_val_.val_int = val; - } - - /** - * @brief Method to call the snmp command. Object type determines what SET command is sent. - * Types are Omit and Hold. - * @return True if SET commands are successful. False if command fails. - * */ - bool run() - { - /*Type of request to be sent to the TSC, within this context it is always SET*/ - request_type type = request_type::SET; - - if(control_type_ == control_type::Omit) - { - if(!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_OMIT_CONTROL, type, set_val_)){ - return false; - } - } - else - { - if(!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_HOLD_CONTROL, type, set_val_)){ - return false; - } - } - - return true; - - } - - /** - * @brief Method to return information about the snmp set command - * @return Returns string formatted as "control_cmd_type:;execution_start_time:;signal_groups_set:". - * */ - std::string get_cmd_info() const; - }; - class control_tsc_state { // This class aims to control the phases on the traffic signal controller using Omit and Hold SNMP commands @@ -95,26 +37,32 @@ namespace traffic_signal_controller_service * @param tsc_command_queue Queue of snmp commands to set HOLD and OMIT on the traffic signal controller **/ void update_tsc_control_queue(std::shared_ptr desired_phase_plan, - std::queue& tsc_command_queue) const; + std::queue& tsc_command_queue) const; /** - * @brief Method to create OMIT snmp command for provided signal groups, which will result in the traffic signal controller skipping the specified phases. - * This method should be sent before any yellow phase for omission to take effect. - * @param signal_groups A list of signal groups NOT to be omitted. Omit command will aim to omit everything besides signal groups specified here. - * @param start_time Time at which the snmp command needs to be sent - * @param is_reset if true, omit command is reset on the traffic signal controller to 0. - * If false will calculate the omit value required to reach given signal groups + * @brief Method to update the queue of tsc_control + * @param phase_control_schedule Pointer to the phase control schedule. + * @param tsc_command_queue Queue of snmp commands to set HOLD and OMIT on the traffic signal controller.This queue is a managed by the tsc_service and passed by reference for update by incoming phase control schedule changes. **/ - snmp_cmd_struct create_omit_command(const std::vector& signal_groups, int64_t start_time, bool is_reset = false) const; + void update_tsc_control_queue(std::shared_ptr phase_control_schedule, + std::queue& tsc_command_queue) const; /** - * @brief Method to create command for Hold for provided signal groups, - * which will result in the traffic signal controller "holding" the specified phases till a change in the Hold command. + * @brief Method to create command for provided signal groups. Calculate the value required to control (hold, forceoff, omit, call) given signal groups. * @param signal_groups A list of signal groups NOT to be omitted. Hold command will aim to hold the signal groups specified here. - * @param start_time Time at which the snmp command needs to be sent - * @param is_reset if true, hold command is reset on the traffic signal controller to 0. - * If false will calculate the value required to hold given signal groups + * @param start_time Time at which the snmp command needs to be sent. **/ - snmp_cmd_struct create_hold_command(const std::vector& signal_groups, int64_t start_time, bool is_reset = false) const; + streets_snmp_cmd::snmp_cmd_struct create_snmp_command_by_signal_groups(const std::vector& signal_groups, streets_snmp_cmd::PHASE_CONTROL_TYPE phase_control_type, int64_t start_time) const; + /** + * @brief Method to call the snmp command. Object type determines what SET command is sent. + * Types are Omit, Forceoff, Call and Hold. + * @return True if SET commands are successful. False if command fails. + * */ + bool run_snmp_cmd_set_request(streets_snmp_cmd::snmp_cmd_struct& snmp_cmd) const; + /** + * @brief Method to call the vector of snmp commands. Object type determines what SET command is sent. + * Types are Omit, Forceoff, Call and Hold. + * */ + void run_clear_all_snmp_commands() const; }; } \ No newline at end of file diff --git a/tsc_client_service/include/mock_snmp_client.h b/tsc_client_service/include/mock_snmp_client.h index b37562f04..643ae0778 100644 --- a/tsc_client_service/include/mock_snmp_client.h +++ b/tsc_client_service/include/mock_snmp_client.h @@ -20,7 +20,7 @@ namespace traffic_signal_controller_service { */ mock_snmp_client(const std::string& ip = "", const int &port = 0 ) : snmp_client(ip, port){}; ~mock_snmp_client() = default; - MOCK_METHOD(bool, process_snmp_request, (const std::string &input_oid, const request_type &request_type, snmp_response_obj &val), (override)); + MOCK_METHOD(bool, process_snmp_request, (const std::string &input_oid, const streets_snmp_cmd::REQUEST_TYPE &request_type, streets_snmp_cmd::snmp_response_obj &val), (override)); }; } \ No newline at end of file diff --git a/tsc_client_service/include/monitor_tsc_state.h b/tsc_client_service/include/monitor_tsc_state.h index a8ea722e1..f6564165a 100644 --- a/tsc_client_service/include/monitor_tsc_state.h +++ b/tsc_client_service/include/monitor_tsc_state.h @@ -59,6 +59,10 @@ namespace traffic_signal_controller_service /* The sequence of vehicle phases in ring 2 of TSC*/ std::vector phase_seq_ring2_; + std::vector vehicle_calls; + + std::vector pedestrian_calls; + // Number of required following movements on receiving a spat_ptr int required_following_movements_ = 3; @@ -107,6 +111,7 @@ namespace traffic_signal_controller_service **/ std::vector> get_active_ring_sequences(int max_rings, std::unordered_map& vehicle_phase_2signalgroup_map, int sequence = 1) const; + /** * @brief Method for mapping vehicle phases and signal groups. Modifies non-const arguments by reference. * Signal group map is expected to be passed empty. @@ -119,6 +124,24 @@ namespace traffic_signal_controller_service void map_phase_and_signalgroup(const std::vector>& active_ring_sequences, std::unordered_map& vehicle_phase_2signalgroup_map, std::unordered_map& signal_group_2vehiclephase_map) const; + /** + * @brief Method to process bitwise response from NTCIP 1202 Phase Status Group Table. In this table phase information + * is returned as a single 8 bit integer and can be intepreted as follows: + * ``` + * Bit 7: Phase # = (phaseStatusGroupNumber * 8) + * Bit 6: Phase # = (phaseStatusGroupNumber * 8) - 1 + * Bit 5: Phase # = (phaseStatusGroupNumber * 8) - 2 + * Bit 4: Phase # = (phaseStatusGroupNumber * 8) - 3 + * Bit 3: Phase # = (phaseStatusGroupNumber * 8) - 4 + * Bit 2: Phase # = (phaseStatusGroupNumber * 8) - 5 + * Bit 1: Phase # = (phaseStatusGroupNumber * 8) - 6 + * Bit 0: Phase # = (phaseStatusGroupNumber * 8) - 7 + * ``` + * @param resp + * @param offset + * @return + */ + std::vector process_bitwise_response( const streets_snmp_cmd::snmp_response_obj &resp, int offset ) const; /** * @brief Method for getting maximum channels for the traffic signal controller * @return number of maximum channels in the traffic signal controller @@ -172,7 +195,7 @@ namespace traffic_signal_controller_service * @param ring_num The phase for which the concurrent phases needs to be obtained * @return a vector of phases that may be concurrent with the given phase **/ - std::vector get_concurrent_signal_groups(int phase_num); + std::vector get_concurrent_signal_groups(int phase_num) const; /** @brief Get predicted next movement event given a current event * @param current_event movement_event from the next movement needs to be predicted @@ -182,6 +205,19 @@ namespace traffic_signal_controller_service **/ signal_phase_and_timing::movement_event get_following_event(const signal_phase_and_timing::movement_event& current_event, uint64_t current_event_end_time, const signal_group_state& phase_state) const; + + /** + * @brief Helper method to convert phase numbers to signal groups ids for a vector of vehicle phases. + * @param veh_phases vector of vehicle phase numbers. + * @return vector of vehicle signal group ids. + */ + std::vector convert_veh_phases_to_signal_groups(const std::vector &veh_phases ) const; + /** + * @brief Helper method to convert phase numbers to signal group ids for a vector of pedestrian phases. + * @param ped_phases vector of pedestrian phase numbers. + * @return vector pedestrian signal group ids. + */ + std::vector convert_ped_phases_to_signal_groups(const std::vector &ped_phases ) const; //Add Friend Test to share private members FRIEND_TEST(test_monitor_state, test_get_following_movement_events); @@ -212,11 +248,27 @@ namespace traffic_signal_controller_service const std::unordered_map & get_ped_phase_map(); /** - * @brief Returns a map of pedestrian phases to signal group ids - * @return a map of pedestrian phases to signal group ids + * @brief Returns a map of vehicle phases to signal group ids + * @return a map of vehicle phases to signal group ids **/ const std::unordered_map& get_vehicle_phase_map(); + /** + * @brief Return a map of signal group ids to phases map. + * @return a map of signal group ids to vehicle phases map. + */ + const std::unordered_map & get_signal_group_map(); + + std::vector get_vehicle_calls() const; + + std::vector get_pedestrian_calls() const; + + std::string vector_to_string(const std::vector &v ) const; + /** + * @brief Poll vehicle/pedestrian calls on phases 1-16 + */ + void poll_vehicle_pedestrian_calls(); + /** * @brief Get the phase number using signal group id. * @@ -233,7 +285,9 @@ namespace traffic_signal_controller_service * @return int * @throws monitor_states_exception if phase number is less than 1. */ - int get_vehicle_signal_group_id(const int phase_number); + int get_vehicle_signal_group_id(const int phase_number) const; + + int get_pedestrian_signal_group_id(const int phase_number) const; /** * @brief Initialize tsc_state by making SNMP calls to TSC for phase sequence and timing information. diff --git a/tsc_client_service/include/ntcip_oids.h b/tsc_client_service/include/ntcip_oids.h index 3fe87bfaf..835110cff 100644 --- a/tsc_client_service/include/ntcip_oids.h +++ b/tsc_client_service/include/ntcip_oids.h @@ -77,7 +77,7 @@ namespace ntcip_oids { static const std::string PHASE_CONCURRENCY = "1.3.6.1.4.1.1206.4.2.1.1.2.1.23"; /** - * @brief Phase Omit Control object is used to allow omission of pedestrian phases from being serviced in the device. + * @brief Phase Omit Control object is used to allow omission of vehicle phases from being serviced in the device. * It takes in a 8 bit argument, one for each phase. When a bit=1 the Traffic Signal Controller will omit * that phase and keep omitting till the bit is set to 0. * Values range from 0-255. @@ -86,13 +86,33 @@ namespace ntcip_oids { static const std::string PHASE_OMIT_CONTROL = "1.3.6.1.4.1.1206.4.2.1.1.5.1.2.1"; /** - * @brief Phase Hold Control object is used to Hold pedestrian phases. + * @brief Phase Omit Control object is used to allow omission of pedestrian phases from being serviced in the device. + * It takes in a 8 bit argument, one for each phase. When a bit=1 the Traffic Signal Controller will omit + * that phase and keep omitting till the bit is set to 0. + * Values range from 0-255. + * .1 is needed at the end to set the phase control. NTCIP documentation is unclear about why its required or what it represents + */ + static const std::string PHASE_PEDESTRIAN_OMIT_CONTROL = "1.3.6.1.4.1.1206.4.2.1.1.5.1.3.1"; + + /** + * @brief Phase Hold Control object is used to Hold Vehicle phases. * It takes in a 8 bit argument, one for each phase. When a bit=1 the Traffic Signal Controller will hold * that phase when it turns green and keep holding till the bit is set to 0. * Values range from 0-255. * .1 is needed at the end to set the phase control. NTCIP documentation is unclear about why its required or what it represents */ static const std::string PHASE_HOLD_CONTROL = "1.3.6.1.4.1.1206.4.2.1.1.5.1.4.1"; + /** + * @brief Phase Vehicle Call Status Mask, when a bit =1, the Phase vehicle currently has a call for service. When a bit + * = 0, the Phase vehicle currently does NOT have a call for service. + */ + static const std::string PHASE_STATUS_GROUP_VEH_CALLS ="1.3.6.1.4.1.1206.4.2.1.1.4.1.8"; + /** + * @brief Phase Pedestrian Call Status Mask, when a bit= 1, the Phase pedestrian currently has a call for service. When + * a bit = 0, the Phase pedestrian currently does NOT have a call for service. + */ + static const std::string PHASE_STATUS_GROUP_PED_CALLS ="1.3.6.1.4.1.1206.4.2.1.1.4.1.9"; + /** * @brief Phase Status Group Phase Next object is used to determine whether any vehicle phases are committed by the * TSC to be served next. It returns a 8 bit value that represent a state for each of the 8 vehicle phases. When a bit = 1, @@ -104,4 +124,28 @@ namespace ntcip_oids { */ static const std::string PHASE_STATUS_GROUP_PHASE_NEXT = "1.3.6.1.4.1.1206.4.2.1.1.4.1.11.1"; + /** + * @brief Phase FORCEOFF Control object is used to allow apply force offs on a per phase basis. + * It takes in a 8 bit argument, one for each phase. When a bit=1 the Traffic Signal Controller will force off + * that phase, and should not activate the system phase force off control for that phase when the bit is set to 0. + * Values range from 0-255. When the phase green terminates, the associated bit shall be reset to 0. + * .1 is needed at the end to set the phase control. NTCIP documentation is unclear about why its required or what it represents + */ + static const std::string PHASE_FORCEOFF_CONTROL = "1.3.6.1.4.1.1206.4.2.1.1.5.1.5.1"; + + /** + * @brief Phase vehicle call control object is used to allow a remote entity to place calls for vehicle service in the service. + * When a bit = 1, the device shall place a call for vehicle service on that phase. + * When a bit = 0, the device shall not place a call for vehicle service on that phase. + * .1 is needed at the end to set the phase control. NTCIP documentation is unclear about why its required or what it represents + */ + static const std::string PHASE_VEHICLE_CALL_CONTROL = "1.3.6.1.4.1.1206.4.2.1.1.5.1.6.1"; + + /** + * @brief The phase pedestrian call control is used to allow a remote entity to place calls for ped service in the device. + * When a bit = 1, the device shall place a call for ped service on that phase. + * When a bit = 0, the device shall not place a call for ped service on that phase. + */ + static const std::string PHASE_PEDESTRIAN_CALL_CONTROL = "1.3.6.1.4.1.1206.4.2.1.1.5.1.7.1"; + } \ No newline at end of file diff --git a/tsc_client_service/include/snmp_client.h b/tsc_client_service/include/snmp_client.h index f11d09845..89215d614 100644 --- a/tsc_client_service/include/snmp_client.h +++ b/tsc_client_service/include/snmp_client.h @@ -10,40 +10,12 @@ #include "ntcip_oids.h" #include "snmp_client_exception.h" +#include "streets_snmp_cmd.h" namespace traffic_signal_controller_service { -enum class request_type -{ - GET, - SET, - OTHER //Processing this request type is not a defined behavior, included for testing only -}; - -/** @brief A struct to hold the value being sent to the TSC, can be integer or string. Type needs to be defined*/ -struct snmp_response_obj -{ - /** @brief The type of value being requested or set, on the TSC */ - enum class response_type - { - INTEGER, - STRING - }; - - //snmp response values can be any asn.1 supported types. - //Integer and string values can be processed here - int64_t val_int = 0; - std::vector val_string; - response_type type; - - inline bool operator==(const snmp_response_obj& obj2) const - { - return val_int == obj2.val_int && val_string == obj2.val_string && type == obj2.type; - } -}; - class snmp_client { private: @@ -92,19 +64,19 @@ class snmp_client /** @brief Returns true or false depending on whether the request could be processed for given input OID at the Traffic Signal Controller. * @param input_oid The OID to request information for. - * @param request_type The request type for which the error is being logged. Accepted values are "GET" and "SET" only. + * @param streets_snmp_cmd::REQUEST_TYPE The request type for which the error is being logged. Accepted values are "GET" and "SET" only. * @param value_int The integer value for the object returned by reference. For "SET" it is the value to be set. * For "GET", it is the value returned for the returned object by reference. * This is an optional argument, if not provided, defaults to 0. * @param value_str String value for the object, returned by reference. Optional argument, if not provided the value is set as an empty string * @return Integer value at the oid, returns false if value cannot be set/requested or oid doesn't have an integer value to return.*/ - virtual bool process_snmp_request(const std::string& input_oid, const request_type& request_type, snmp_response_obj& val); + virtual bool process_snmp_request(const std::string& input_oid, const streets_snmp_cmd::REQUEST_TYPE& request_type, streets_snmp_cmd::snmp_response_obj& val); /** @brief Finds error type from status and logs an error. * @param status The integer value corresponding to net-snmp defined errors. macros considered are STAT_SUCCESS(0) and STAT_TIMEOUT(2) - * @param request_type The request type for which the error is being logged (GET/SET). + * @param streets_snmp_cmd::REQUEST_TYPE The request type for which the error is being logged (GET/SET). * @param response The snmp_pdu struct */ - void log_error(const int& status, const request_type& request_type, snmp_pdu *response) const; + void log_error(const int& status, const streets_snmp_cmd::REQUEST_TYPE& request_type, snmp_pdu *response) const; /** @brief Destructor for client. Closes the snmp session**/ virtual ~snmp_client(); diff --git a/tsc_client_service/include/spat_projection_mode.h b/tsc_client_service/include/spat_projection_mode.h new file mode 100644 index 000000000..9518357d4 --- /dev/null +++ b/tsc_client_service/include/spat_projection_mode.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +namespace traffic_signal_controller_service{ + /** + * @brief Enumeration to describe how we project future events on to the + * SPAT information received from the Traffic Signal Controller. + */ + enum class SPAT_PROJECTION_MODE { + /** + * @brief Do not append any future information to received SPAT. + */ + NO_PROJECTION = 0, + /** + * @brief Append future information reflected in received DPP + */ + DPP_PROJECTION = 1, + /** + * @brief Append future information assuming fixed time, min vehicle recall on all vehicle phases. + */ + FIXED_TIMING_PROJECTION = 2, + // TODO: Remove if no longer necessary for TM/TSP MMITSS Integration + // /** + // * @brief Append future information assuming reflected in received MRP (MMITSS Road Side Processor) Phase Schedule. + // */ + // MMITSS_PHASE_SCHEDULE = 3 + // ------------------------------------------------------------------------------------- + }; + + SPAT_PROJECTION_MODE spat_projection_mode_from_int( const int i ); +} \ No newline at end of file diff --git a/tsc_client_service/include/tsc_service.h b/tsc_client_service/include/tsc_service.h index db8625594..ac8e6f9c5 100644 --- a/tsc_client_service/include/tsc_service.h +++ b/tsc_client_service/include/tsc_service.h @@ -22,6 +22,7 @@ #include #include "streets_service.h" #include "streets_clock_singleton.h" +#include "spat_projection_mode.h" namespace traffic_signal_controller_service { @@ -40,6 +41,11 @@ namespace traffic_signal_controller_service { */ std::shared_ptr desired_phase_plan_consumer; + /* + * @brief Kafka consumer for consuming Phase Control Schedule JSON + */ + std::shared_ptr phase_control_schedule_consumer; + /** * @brief spat_worker contains udp_socket_listener and consumes UDP data * packets and updates spat accordingly. @@ -66,6 +72,11 @@ namespace traffic_signal_controller_service { * JSON message. */ std::shared_ptr spat_ptr; + + /** + * @brief Point to phase control schedule object which is updated based on the received JSON message from MMITSS Road side processor. + */ + std::shared_ptr phase_control_schedule_ptr; /** * @brief Pointer to tsc_configuration_state object which is traffic signal controller * configuration information obtained from the tsc_state worker @@ -91,10 +102,10 @@ namespace traffic_signal_controller_service { int tsc_config_send_attempts = 1; // desired phase plan information consumed from desire_phase_plan Kafka topic - bool use_desired_phase_plan_update_ = false; + SPAT_PROJECTION_MODE spat_proj_mode = SPAT_PROJECTION_MODE::NO_PROJECTION; // Queue to store snmp_cmd_structs which are objects used to run snmp HOLD and OMIT commands - std::queue tsc_set_command_queue_; + std::queue tsc_set_command_queue_; // Configurable sleep duration for control_tsc_state thread in milliseconds. This sleep is required to allow some time between checking queue for control commands int control_tsc_state_sleep_dur_ = 0; @@ -102,11 +113,24 @@ namespace traffic_signal_controller_service { // Configurable parameter that is used to enable logging of snmp commands to a log file if set to true. bool enable_snmp_cmd_logging_ = false; + inline static const std::string SNMP_COMMAND_LOGGER_NAME = "snmp_command"; + + inline static const std::string VEH_PED_CALL_LOGGER_NAME = "veh_ped_call"; + //Add Friend Test to share private members friend class tsc_service_test; FRIEND_TEST(tsc_service_test,test_tsc_control); FRIEND_TEST(tsc_service_test,test_produce_tsc_config_json_timeout); FRIEND_TEST(tsc_service_test,test_init_kafka_consumer_producer); + FRIEND_TEST(tsc_service_test,test_configure_snmp_cmd_logger); + FRIEND_TEST(tsc_service_test,test_configure_veh_ped_call_logger); + + /*** + * Configuration parameter to control different interfaces to schedule the traffic signal controller + * If false, it will use carma-streets internal signal optimization service and its generated desired phase plan to schedule traffic signal controller. + * If true, it will use external MRP (MMITSS Roadside Processor, link: https://github.com/mmitss/mmitss-az) and its generated phase control schedule to schedule the traffic sginal controller. + * **/ + bool use_mmitss_mrp = false; public: @@ -151,6 +175,7 @@ namespace traffic_signal_controller_service { * @return false if initialization is not successful. */ bool initialize_tsc_state( const std::shared_ptr _snmp_client_ptr); + /** * @brief Method to enable spat using the TSC Service SNMP Client. * @@ -210,22 +235,33 @@ namespace traffic_signal_controller_service { void produce_tsc_config_json() const; void consume_desired_phase_plan(); + /** + * @brief Thread callback function to run the phase control schedule Kafka consumer to consume phase control schedule JSON message + */ + void consume_phase_control_schedule(); /** * @brief Method to control phases on the Traffic Signal Controller by sending OMIT and HOLD commands constructed to - * follow the desired phase plan. Calls set_tsc_hold_and_omit + * follow the desired phase plan. Calls set_tsc_hold_and_omit_forceoff_call **/ void control_tsc_phases(); /** - * @brief Method to set HOLD and OMIT on the Traffic signal controller accorording to the desired phase plan. + * @brief Method to set HOLD and OMIT, CALL, FORCEOFF on the Traffic signal controller accorording to the desired phase plan/phase control schedule. **/ - void set_tsc_hold_and_omit(); + void set_tsc_hold_and_omit_forceoff_call(); /** - * @brief Method to configure spdlog::logger for logging snmp control commands into daily rotating csv file. + * @brief Method to configure spdlog::logger for logging snmp control commands into daily rotating log file. */ void configure_snmp_cmd_logger() const; + /** + * @brief ethod to configure spdlog::logger for logging vehicle and pedestrian calls on the traffic signal controller + * into daily rotating csvlog file. + * + */ + void configure_veh_ped_call_logger() const; + /** * @brief Method to log spat latency comparing spat time stamp to current time. Calculates and average over 20 * messages then resets count and spat_processing_time. diff --git a/tsc_client_service/manifest.json b/tsc_client_service/manifest.json index f6ead6a43..065be9edf 100644 --- a/tsc_client_service/manifest.json +++ b/tsc_client_service/manifest.json @@ -66,6 +66,12 @@ "description": "Kafka topic for streets internal SPAT messages", "type": "STRING" }, + { + "name": "phase_control_schedule_consumer_topic", + "value": "phase_control_schedule", + "description": "Kafka topic for streets internal phase control schedule messages", + "type": "STRING" + }, { "name": "desired_phase_plan_consumer_topic", "value": "desired_phase_plan", @@ -85,10 +91,10 @@ "type": "BOOL" }, { - "name": "use_desired_phase_plan_update", - "value": true, - "description": "If true will enable monitor_desired_phase_plan to update incoming spat with calculated future movement events, using desired phase plan information from signal optimization service. If false will use TSC Configuration to predict future phases and append those to the spat based on default phase sequence.", - "type": "BOOL" + "name": "spat_projection_mode", + "value": 0, + "description": "Enumeration configuring SPat projection. 1 == DPP projection. 2 == Fixed Timing projection. Any other value will result in no projection of future", + "type": "INTEGER" }, { "name": "tsc_config_producer_topic", @@ -119,6 +125,12 @@ "value": "snmpcmdLogs", "description": "Filename for snmp commands logging (Note: set enable_snmp_cmd_logging true).", "type": "STRING" + }, + { + "name": "use_mmitss_mrp", + "value": false, + "description": "If false, it will use carma-streets internal signal optimization service and its generated desired phase plan to schedule traffic signal controller. If true, it will use external MRP (MMITSS Roadside Processor, link: https://github.com/mmitss/mmitss-az) and its generated phase control schedule to schedule the traffic sginal controller.", + "type": "BOOL" } ] diff --git a/tsc_client_service/src/control_tsc_state.cpp b/tsc_client_service/src/control_tsc_state.cpp index 11d74ee9d..c495fe772 100644 --- a/tsc_client_service/src/control_tsc_state.cpp +++ b/tsc_client_service/src/control_tsc_state.cpp @@ -3,46 +3,48 @@ namespace traffic_signal_controller_service { - control_tsc_state::control_tsc_state(std::shared_ptr snmp_client, - std::shared_ptr _tsc_state) - : snmp_client_worker_(snmp_client), _tsc_state(_tsc_state) + control_tsc_state::control_tsc_state(std::shared_ptr snmp_client, + std::shared_ptr _tsc_state) + : snmp_client_worker_(snmp_client), _tsc_state(_tsc_state) { - } void control_tsc_state::update_tsc_control_queue(std::shared_ptr desired_phase_plan, - std::queue& tsc_command_queue) const + std::queue &tsc_command_queue) const { - if(desired_phase_plan->desired_phase_plan.empty()){ + if (desired_phase_plan->desired_phase_plan.empty()) + { SPDLOG_DEBUG("No events in desired phase plan"); return; } - if(desired_phase_plan->desired_phase_plan.size() == 1){ + if (desired_phase_plan->desired_phase_plan.size() == 1) + { SPDLOG_DEBUG("TSC service assumes first event is already set, no update to queue required"); return; } + auto signal_groups_2phase_map = _tsc_state->get_signal_group_map(); // Check if desired phase plan is valid // Check no repeated signal groups in adjacent events - for(int i = 0; i < desired_phase_plan->desired_phase_plan.size() - 1; ++i){ - + for (int i = 0; i < desired_phase_plan->desired_phase_plan.size() - 1; ++i) + { + auto event = desired_phase_plan->desired_phase_plan[0]; auto next_event = desired_phase_plan->desired_phase_plan[1]; - for(auto signal_group : event.signal_groups) + for (auto signal_group : event.signal_groups) { auto it = std::find(next_event.signal_groups.begin(), next_event.signal_groups.end(), signal_group); - if(it != next_event.signal_groups.end()) + if (it != next_event.signal_groups.end()) { SPDLOG_ERROR("TSC Service assumes adjacent events dont have the same signal_group. Given Desired phase plan fails this assumption"); throw control_tsc_state_exception("Repeating signal group found in adjacent events. Desired phase plan cannot be set"); } } } - - //Reset queue - tsc_command_queue = std::queue(); + // Reset queue + tsc_command_queue = std::queue(); // add Omit and Hold commands auto first_event = desired_phase_plan->desired_phase_plan[0]; @@ -50,109 +52,166 @@ namespace traffic_signal_controller_service auto current_time = streets_service::streets_clock_singleton::time_in_ms(); // Assuming the first event start doesn't need to be planned for, we execute omit and hold for the next event. Resetting Hold ends the first event - int64_t omit_execution_time = current_time + (first_event.end_time - current_time)/2; - tsc_command_queue.push(create_omit_command(second_event.signal_groups, omit_execution_time)); + int64_t omit_execution_time = current_time + (first_event.end_time - current_time) / 2; + // Omit all others signal groups other than the second_event event signal_groups + std::vector second_event_signal_groups_to_omit; + for (auto &[sg, phase] : signal_groups_2phase_map) + { + // If the signal group is not in the second_event signal groups, add the signal group to the list of omit signal groups + if (std::find(second_event.signal_groups.begin(), second_event.signal_groups.end(), sg) == second_event.signal_groups.end()) + { + second_event_signal_groups_to_omit.push_back(sg); + } + } + tsc_command_queue.push(create_snmp_command_by_signal_groups(second_event_signal_groups_to_omit, streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, omit_execution_time)); int64_t hold_execution_time = first_event.end_time; - tsc_command_queue.push(create_hold_command(second_event.signal_groups, hold_execution_time)); - + tsc_command_queue.push(create_snmp_command_by_signal_groups(second_event.signal_groups, streets_snmp_cmd::PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, hold_execution_time)); int event_itr = 1; - - while(event_itr < desired_phase_plan->desired_phase_plan.size() - 1) + while (event_itr < desired_phase_plan->desired_phase_plan.size() - 1) { - auto current_event = desired_phase_plan->desired_phase_plan[event_itr]; + auto current_event = desired_phase_plan->desired_phase_plan[event_itr]; auto next_event = desired_phase_plan->desired_phase_plan[event_itr + 1]; - omit_execution_time = current_event.start_time + (current_event.end_time - current_event.start_time)/2; - tsc_command_queue.push(create_omit_command(next_event.signal_groups, omit_execution_time)); + omit_execution_time = current_event.start_time + (current_event.end_time - current_event.start_time) / 2; + // Omit all others signal groups other than the next_event event signal_groups + std::vector next_event_signal_groups_to_omit; + for (auto &[sg, phase] : signal_groups_2phase_map) + { + // If the signal group is not in the next_event signal groups, add the signal group to the list of omit signal groups + if (std::find(next_event.signal_groups.begin(), next_event.signal_groups.end(), sg) == next_event.signal_groups.end()) + { + next_event_signal_groups_to_omit.push_back(sg); + } + } + tsc_command_queue.push(create_snmp_command_by_signal_groups(next_event_signal_groups_to_omit, streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, omit_execution_time)); hold_execution_time = current_event.end_time; - tsc_command_queue.push(create_hold_command(next_event.signal_groups, hold_execution_time)); + tsc_command_queue.push(create_snmp_command_by_signal_groups(next_event.signal_groups, streets_snmp_cmd::PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, hold_execution_time)); event_itr++; } // Reset Hold and Omit for last event auto last_event = desired_phase_plan->desired_phase_plan.back(); - omit_execution_time = last_event.start_time + (last_event.end_time - last_event.start_time)/2; - std::vector empty_group = {}; - tsc_command_queue.push(create_omit_command(empty_group, omit_execution_time, true)); + omit_execution_time = last_event.start_time + (last_event.end_time - last_event.start_time) / 2; + streets_snmp_cmd::streets_snmp_cmd_converter converter; + tsc_command_queue.push(converter.create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, omit_execution_time)); hold_execution_time = last_event.end_time; - tsc_command_queue.push(create_hold_command(empty_group, hold_execution_time, true)); + tsc_command_queue.push(converter.create_snmp_reset_command(streets_snmp_cmd::PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, hold_execution_time)); SPDLOG_DEBUG("Updated queue with front command {0}!", tsc_command_queue.front().get_cmd_info()); - } - snmp_cmd_struct control_tsc_state::create_omit_command(const std::vector& signal_groups, int64_t start_time, bool is_reset) const + void control_tsc_state::update_tsc_control_queue(std::shared_ptr phase_control_schedule_ptr, + std::queue &tsc_command_queue) const { - if(!is_reset) + if (!phase_control_schedule_ptr) { - uint8_t omit_val = 255; //Initialize to 11111111 - - for(auto signal_group : signal_groups) - { - int phase = _tsc_state->get_phase_number(signal_group); - // Omit all phases except the ones in the given movement group - // For Omit only given phase bits are 0. Subtract 1 since phases range from 1-8. - omit_val &= ~(1 << (phase - 1)); - - } + SPDLOG_ERROR("Phase control schedule is not initialized."); + return; + } - snmp_cmd_struct command(snmp_client_worker_, start_time, snmp_cmd_struct::control_type::Omit, static_cast(omit_val)); - return command; + if (phase_control_schedule_ptr->is_clear_current_schedule) + { + //Clear all phase controls from the traffic signal controller + SPDLOG_DEBUG("Clear all phase controls from TSC!"); + run_clear_all_snmp_commands(); + + SPDLOG_INFO("Clear SNMP command queue!"); + // Clear all commands on the update command queue. + std::queue empty_queue; + std::swap(tsc_command_queue, empty_queue); } else { - snmp_cmd_struct command(snmp_client_worker_, start_time, snmp_cmd_struct::control_type::Omit, static_cast(0)); - return command; + std::stringstream ss; + ss << *phase_control_schedule_ptr; + SPDLOG_DEBUG("Update SNMP command queue with new phase control schedule commands: {0}", ss.str()); + // Update command queue with the new phase control schedule commands. + streets_snmp_cmd::streets_snmp_cmd_converter converter; + auto snmp_cmd_queue = converter.create_snmp_cmds_by_phase_control_schedule(phase_control_schedule_ptr); + std::swap(tsc_command_queue, snmp_cmd_queue); } + SPDLOG_INFO("Updated Queue Size: {0}", tsc_command_queue.size()); + } + streets_snmp_cmd::snmp_cmd_struct control_tsc_state::create_snmp_command_by_signal_groups(const std::vector &signal_groups, streets_snmp_cmd::PHASE_CONTROL_TYPE phase_control_type, int64_t start_time) const + { + streets_snmp_cmd::streets_snmp_cmd_converter converter; + std::vector phases; + for (auto signal_group : signal_groups) + { + int phase = _tsc_state->get_phase_number(signal_group); + phases.push_back(phase); + } + auto command = converter.create_snmp_command_by_phases(phases, phase_control_type, start_time); + return command; } - snmp_cmd_struct control_tsc_state::create_hold_command(const std::vector& signal_groups, int64_t start_time, bool is_reset) const + bool control_tsc_state::run_snmp_cmd_set_request(streets_snmp_cmd::snmp_cmd_struct &snmp_cmd) const { - if(!is_reset) + if (!snmp_client_worker_) { - uint8_t hold_val = 0; //Initialize to 00000000 + throw control_tsc_state_exception("SNMP Client worker is not initialized."); + } + /*Type of request to be sent to the TSC, within this context it is always SET*/ + auto type = streets_snmp_cmd::REQUEST_TYPE::SET; - for(auto signal_group : signal_groups) + if (snmp_cmd.control_type_ == streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_VEH_PHASES) + { + if (!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_OMIT_CONTROL, type, snmp_cmd.set_val_)) { - int phase = _tsc_state->get_phase_number(signal_group); - // Hold phases in the given movement group - //For Hold only given phase bits are 1. Subtract 1 since phases range from 1-8. - hold_val |= (1 << ( phase - 1)); - + return false; } - - snmp_cmd_struct command(snmp_client_worker_, start_time, snmp_cmd_struct::control_type::Hold, static_cast(hold_val)); - return command; } - else + else if (snmp_cmd.control_type_ == streets_snmp_cmd::PHASE_CONTROL_TYPE::HOLD_VEH_PHASES) { - snmp_cmd_struct command(snmp_client_worker_, start_time, snmp_cmd_struct::control_type::Hold, static_cast(0)); - return command; + if (!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_HOLD_CONTROL, type, snmp_cmd.set_val_)) + { + return false; + } } - } - - std::string snmp_cmd_struct::get_cmd_info() const{ - - std::string command_type; - if(control_type_ == control_type::Hold){ - command_type = "Hold"; + else if (snmp_cmd.control_type_ == streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_PED_PHASES) + { + if (!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_PEDESTRIAN_OMIT_CONTROL, type, snmp_cmd.set_val_)) + { + return false; + } } - else{ - command_type = "Omit"; + else if (snmp_cmd.control_type_ == streets_snmp_cmd::PHASE_CONTROL_TYPE::FORCEOFF_PHASES) + { + if (!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_FORCEOFF_CONTROL, type, snmp_cmd.set_val_)) + { + return false; + } + } + else if (snmp_cmd.control_type_ == streets_snmp_cmd::PHASE_CONTROL_TYPE::CALL_PED_PHASES) + { + if (!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_PEDESTRIAN_CALL_CONTROL, type, snmp_cmd.set_val_)) + { + return false; + } + } + else if (snmp_cmd.control_type_ == streets_snmp_cmd::PHASE_CONTROL_TYPE::CALL_VEH_PHASES) + { + if (!snmp_client_worker_->process_snmp_request(ntcip_oids::PHASE_VEHICLE_CALL_CONTROL, type, snmp_cmd.set_val_)) + { + return false; + } } - std::string execution_start_time = std::to_string(start_time_); - - // Convert value set to phases nums - std::string value_set = std::to_string(set_val_.val_int); - return "control_cmd_type:" + command_type + ";execution_start_time:" + execution_start_time + ";value_set:" + value_set; - + return true; } - + void control_tsc_state::run_clear_all_snmp_commands() const + { + streets_snmp_cmd::streets_snmp_cmd_converter converter; + auto snmp_cmds = converter.create_clear_all_snmp_commands(0);//Set default execution time 0 because it is executed immediately at following line + for(auto& snmp_cmd: snmp_cmds) + { + run_snmp_cmd_set_request(snmp_cmd); + } + } } \ No newline at end of file diff --git a/tsc_client_service/src/monitor_desired_phase_plan.cpp b/tsc_client_service/src/monitor_desired_phase_plan.cpp index 04c929f4f..1ff5700f7 100644 --- a/tsc_client_service/src/monitor_desired_phase_plan.cpp +++ b/tsc_client_service/src/monitor_desired_phase_plan.cpp @@ -142,9 +142,9 @@ namespace traffic_signal_controller_service } } // Once start time is determine. We must determine next phase. - snmp_response_obj response; - response.type = snmp_response_obj::response_type::INTEGER; - _snmp_client->process_snmp_request(ntcip_oids::PHASE_STATUS_GROUP_PHASE_NEXT,traffic_signal_controller_service::request_type::GET, response ); + streets_snmp_cmd::snmp_response_obj response; + response.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + _snmp_client->process_snmp_request(ntcip_oids::PHASE_STATUS_GROUP_PHASE_NEXT, streets_snmp_cmd::REQUEST_TYPE::GET, response ); // Generate Desired Phase Plan with next green fixed. streets_desired_phase_plan::streets_desired_phase_plan one_fixed_green; diff --git a/tsc_client_service/src/monitor_tsc_state.cpp b/tsc_client_service/src/monitor_tsc_state.cpp index 6f989adde..62cffefb7 100644 --- a/tsc_client_service/src/monitor_tsc_state.cpp +++ b/tsc_client_service/src/monitor_tsc_state.cpp @@ -149,6 +149,115 @@ namespace traffic_signal_controller_service return next_event; } + std::string tsc_state::vector_to_string(const std::vector &v) const { + std::string v_string = "["; + for (auto element: v ){ + v_string.append(std::to_string(element)); + if ( element != v.back()) { + v_string.append(", "); + } + } + v_string.append("]"); + return v_string; +} + + void tsc_state::poll_vehicle_pedestrian_calls() { + // Make SNMP requests to get vehicle and pedestrian calls + // for phases 1-16 (.1 gets 1-8, .2 gets 9-16) + auto request_type = streets_snmp_cmd::REQUEST_TYPE::GET; + std::string vehicle_call_phases_1_8 = ntcip_oids::PHASE_STATUS_GROUP_VEH_CALLS + ".1"; + std::string vehicle_call_phases_9_16 = ntcip_oids::PHASE_STATUS_GROUP_VEH_CALLS + ".2"; + std::string pedestrian_call_phases_1_8 = ntcip_oids::PHASE_STATUS_GROUP_PED_CALLS + ".1"; + std::string pedestrian_call_phases_9_16 = ntcip_oids::PHASE_STATUS_GROUP_PED_CALLS + ".2"; + + streets_snmp_cmd::snmp_response_obj veh_call_1_8; + veh_call_1_8.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + streets_snmp_cmd::snmp_response_obj veh_call_9_16; + veh_call_9_16.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + streets_snmp_cmd::snmp_response_obj ped_call_1_8; + ped_call_1_8.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + streets_snmp_cmd::snmp_response_obj ped_call_9_16; + ped_call_9_16.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + + snmp_client_worker_ ->process_snmp_request(vehicle_call_phases_1_8, request_type, veh_call_1_8); + snmp_client_worker_ ->process_snmp_request(vehicle_call_phases_9_16, request_type, veh_call_9_16); + snmp_client_worker_ ->process_snmp_request(pedestrian_call_phases_1_8, request_type, ped_call_1_8); + snmp_client_worker_ ->process_snmp_request(pedestrian_call_phases_9_16, request_type, ped_call_9_16); + + SPDLOG_INFO("Response Veh {0}, {1} Response Ped {2}, {3}", veh_call_1_8.val_int, veh_call_9_16.val_int, ped_call_1_8.val_int, ped_call_9_16.val_int); + + // Process returned int bitwise to get vehicle/pedestrian call information for 8 phases and convert to signal groups + auto veh_resp_1_8 = convert_veh_phases_to_signal_groups( process_bitwise_response(veh_call_1_8,0)); + auto veh_resp_9_16 = convert_veh_phases_to_signal_groups(process_bitwise_response(veh_call_9_16,8)); + auto ped_resp_1_8 = convert_ped_phases_to_signal_groups(process_bitwise_response(ped_call_1_8,0)); + auto ped_resp_9_16 = convert_ped_phases_to_signal_groups(process_bitwise_response(ped_call_9_16,8)); + + // Clear previously saved vehicle/pedestrian information and replace with new information + + vehicle_calls.clear(); + vehicle_calls.insert(vehicle_calls.end(), veh_resp_1_8.begin(), veh_resp_1_8.end()); + vehicle_calls.insert(vehicle_calls.end(), veh_resp_9_16.begin(), veh_resp_9_16.end()); + + pedestrian_calls.clear(); + pedestrian_calls.insert(pedestrian_calls.end(), ped_resp_1_8.begin(), ped_resp_1_8.end()); + pedestrian_calls.insert(pedestrian_calls.end(), ped_resp_9_16.begin(), ped_resp_9_16.end()); + + SPDLOG_INFO("Pedestrian calls {0}, Vehicle calls, {1}", vector_to_string(pedestrian_calls), vector_to_string(vehicle_calls)); + } + + std::vector tsc_state::process_bitwise_response( const streets_snmp_cmd::snmp_response_obj &response, int offset ) const{ + /** + * Response value is 8 bit int in which each bit is interpreted individually as 1 or 0. 1 + * indicates that the vehicle phase for that bit is committed to be next. 0 indicates this + * phase is not committed to be next. + * + * bit 0 represent vehicle phase 1 + * bit 1 represent vehicle phase 2 + * bit 2 represent vehicle phase 3 + * bit 3 represent vehicle phase 4 + * bit 4 represent vehicle phase 5 + * bit 5 represent vehicle phase 6 + * bit 6 represent vehicle phase 7 + * bit 7 represent vehicle phase 8 + * + */ + std::vector phase_numbers; + for (uint i = 0; i < 8; ++i) { + if ((response.val_int >> i) & 1) { + // Add any signal group for phase that has bit as 1 + auto phase_number = i+1+offset; + SPDLOG_INFO("Adding phase {0}", phase_number); + phase_numbers.push_back(phase_number); + } + } + return phase_numbers; + } + + std::vector tsc_state::get_pedestrian_calls() const { + return pedestrian_calls; + } + + std::vector tsc_state::get_vehicle_calls() const { + return vehicle_calls; + } + + std::vector tsc_state::convert_ped_phases_to_signal_groups( const std::vector &ped_phases ) const{ + std::vector signal_groups; + for (auto ped_phase : ped_phases) { + signal_groups.push_back(get_pedestrian_signal_group_id(ped_phase)); + } + return signal_groups; + } + + std::vector tsc_state::convert_veh_phases_to_signal_groups( const std::vector &veh_phases ) const{ + std::vector signal_groups; + for (auto veh_phase : veh_phases) { + signal_groups.push_back(get_vehicle_signal_group_id(veh_phase)); + } + return signal_groups; + } + + void tsc_state::add_future_movement_events(std::shared_ptr spat_ptr) { // Modify spat according to phase configuration @@ -273,12 +382,12 @@ namespace traffic_signal_controller_service std::unordered_map tsc_state::get_phases_associated_with_channel(const std::vector& signal_group_ids) const { std::unordered_map phases_to_signal_group; - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; for(int signal_group : signal_group_ids) { - snmp_response_obj phase_num; - phase_num.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj phase_num; + phase_num.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; // Control source returns the phase associated with the signal group. Channel and signal group id is synonymous according to the NTCIP documentation std::string control_source_parameter_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(signal_group); snmp_client_worker_->process_snmp_request(control_source_parameter_oid, request_type, phase_num); @@ -302,10 +411,10 @@ namespace traffic_signal_controller_service void tsc_state::get_channels(int max_channels, std::vector& vehicle_channels, std::vector& ped_channels) const{ // Loop through channel control types and add channels with vehicle phase to list - snmp_response_obj control_type; - control_type.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj control_type; + control_type.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; - request_type request_type = request_type::GET; + auto request_type = streets_snmp_cmd::REQUEST_TYPE::GET; for(int channel_num = 1; channel_num <= max_channels; ++channel_num) { std::string control_type_parameter_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(channel_num); @@ -344,7 +453,7 @@ namespace traffic_signal_controller_service std::vector> active_ring_sequences; // Read sequence data for rings - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; for(int i = 1 ; i <= max_rings; ++i){ int ring_num = i; @@ -352,8 +461,8 @@ namespace traffic_signal_controller_service std::string phase_seq_oid= ntcip_oids::SEQUENCE_DATA + "." + std::to_string(sequence) + "." + std::to_string(ring_num); - snmp_response_obj seq_data; - seq_data.type = snmp_response_obj::response_type::STRING; + streets_snmp_cmd::snmp_response_obj seq_data; + seq_data.type = streets_snmp_cmd::RESPONSE_TYPE::STRING; snmp_client_worker_->process_snmp_request(phase_seq_oid, request_type, seq_data); //extract phase numbers from strings @@ -378,9 +487,9 @@ namespace traffic_signal_controller_service } int tsc_state::get_max_rings() const { - request_type request_type = request_type::GET; - snmp_response_obj max_rings_in_tsc; - max_rings_in_tsc.type = snmp_response_obj::response_type::INTEGER; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; + streets_snmp_cmd::snmp_response_obj max_rings_in_tsc; + max_rings_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; if(!snmp_client_worker_->process_snmp_request(ntcip_oids::MAX_RINGS, request_type, max_rings_in_tsc)) { @@ -391,9 +500,9 @@ namespace traffic_signal_controller_service } int tsc_state::get_max_channels() const { - request_type request_type = request_type::GET; - snmp_response_obj max_channels_in_tsc; - max_channels_in_tsc.type = snmp_response_obj::response_type::INTEGER; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; + streets_snmp_cmd::snmp_response_obj max_channels_in_tsc; + max_channels_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; if(!snmp_client_worker_->process_snmp_request(ntcip_oids::MAX_CHANNELS, request_type, max_channels_in_tsc)) { @@ -417,13 +526,27 @@ namespace traffic_signal_controller_service } } - int tsc_state::get_vehicle_signal_group_id(const int phase_number ) { + int tsc_state::get_vehicle_signal_group_id(const int phase_number ) const { if (phase_number >= 1) { if ( vehicle_phase_2signalgroup_map_.find(phase_number) != vehicle_phase_2signalgroup_map_.end() ) { - return vehicle_phase_2signalgroup_map_[phase_number]; + return vehicle_phase_2signalgroup_map_.at(phase_number); + } + else { + throw monitor_states_exception("No signal group id found for vehicle phase number " + std::to_string(phase_number) + "!"); + } + } + else { + throw monitor_states_exception("Phase numbers less than 1 are invalid!"); + } + } + + int tsc_state::get_pedestrian_signal_group_id(const int phase_number) const{ + if (phase_number >= 1) { + if ( ped_phase_2signalgroup_map_.find(phase_number) != ped_phase_2signalgroup_map_.end() ) { + return ped_phase_2signalgroup_map_.at(phase_number); } else { - throw monitor_states_exception("No signal group id found for phase number " + std::to_string(phase_number) + "!"); + throw monitor_states_exception("No signal group id found for pedestrian phase number " + std::to_string(phase_number) + "!"); } } else { @@ -433,11 +556,11 @@ namespace traffic_signal_controller_service int tsc_state::get_min_green(int phase_num) const { - request_type request_type = request_type::GET; + auto request_type = streets_snmp_cmd::REQUEST_TYPE::GET; std::string min_green_parameter_oid = ntcip_oids::MINIMUM_GREEN + "." + std::to_string(phase_num); - snmp_response_obj min_green; - min_green.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj min_green; + min_green.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; snmp_client_worker_->process_snmp_request(min_green_parameter_oid, request_type, min_green); @@ -446,11 +569,11 @@ namespace traffic_signal_controller_service int tsc_state::get_max_green(int phase_num) const { - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; std::string max_green_parameter_oid = ntcip_oids::MAXIMUM_GREEN + "." + std::to_string(phase_num); - snmp_response_obj max_green; - max_green.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj max_green; + max_green.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; snmp_client_worker_->process_snmp_request(max_green_parameter_oid, request_type, max_green); @@ -459,11 +582,11 @@ namespace traffic_signal_controller_service int tsc_state::get_yellow_duration(int phase_num) const { - request_type request_type = request_type::GET; + auto request_type = streets_snmp_cmd::REQUEST_TYPE::GET; std::string yellow_duration_oid = ntcip_oids::YELLOW_CHANGE_PARAMETER + "." + std::to_string(phase_num); - snmp_response_obj yellow_duration; - yellow_duration.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj yellow_duration; + yellow_duration.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; snmp_client_worker_ ->process_snmp_request(yellow_duration_oid, request_type, yellow_duration); @@ -472,11 +595,11 @@ namespace traffic_signal_controller_service int tsc_state::get_red_clearance(int phase_num) const { - request_type get_request = request_type::GET; + auto get_request = streets_snmp_cmd::REQUEST_TYPE::GET; std::string red_clearance_oid = ntcip_oids::RED_CLEAR_PARAMETER + "." + std::to_string(phase_num); - snmp_response_obj red_clearance; - red_clearance.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj red_clearance; + red_clearance.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; snmp_client_worker_->process_snmp_request(red_clearance_oid, get_request, red_clearance); @@ -512,15 +635,15 @@ namespace traffic_signal_controller_service return red_duration; } - std::vector tsc_state::get_concurrent_signal_groups(int phase_num) + std::vector tsc_state::get_concurrent_signal_groups(int phase_num) const { std::vector concurrent_signal_groups; std::string concurrent_phases_oid = ntcip_oids::PHASE_CONCURRENCY + "." + std::to_string(phase_num); - snmp_response_obj concurrent_phase_data; - request_type request_type = request_type::GET; - concurrent_phase_data.type = snmp_response_obj::response_type::STRING; + streets_snmp_cmd::snmp_response_obj concurrent_phase_data; + auto request_type = streets_snmp_cmd::REQUEST_TYPE::GET; + concurrent_phase_data.type = streets_snmp_cmd::RESPONSE_TYPE::STRING; snmp_client_worker_->process_snmp_request(concurrent_phases_oid, request_type, concurrent_phase_data); //extract phase numbers from strings @@ -548,6 +671,11 @@ namespace traffic_signal_controller_service return vehicle_phase_2signalgroup_map_; } + const std::unordered_map & tsc_state::get_signal_group_map() + { + return signal_group_2vehiclephase_map_; + } + std::unordered_map& tsc_state::get_signal_group_state_map() { return signal_group_2tsc_state_map_; diff --git a/tsc_client_service/src/snmp_client.cpp b/tsc_client_service/src/snmp_client.cpp index 9a009ce69..daf5f856c 100644 --- a/tsc_client_service/src/snmp_client.cpp +++ b/tsc_client_service/src/snmp_client.cpp @@ -53,18 +53,18 @@ namespace traffic_signal_controller_service } - bool snmp_client::process_snmp_request(const std::string& input_oid, const request_type& request_type, snmp_response_obj& val){ + bool snmp_client::process_snmp_request(const std::string& input_oid, const streets_snmp_cmd::REQUEST_TYPE& request_type, streets_snmp_cmd::snmp_response_obj& val){ /*Structure to hold response from the remote host*/ snmp_pdu *response; // Create pdu for the data - if (request_type == request_type::GET) + if (request_type == streets_snmp_cmd::REQUEST_TYPE::GET) { SPDLOG_DEBUG("Attemping to GET value for: {0}", input_oid); pdu = snmp_pdu_create(SNMP_MSG_GET); } - else if (request_type == request_type::SET) + else if (request_type == streets_snmp_cmd::REQUEST_TYPE::SET) { SPDLOG_DEBUG("Attemping to SET value for {0}", input_oid, " to {1}", val.val_int); pdu = snmp_pdu_create(SNMP_MSG_SET); @@ -85,17 +85,17 @@ namespace traffic_signal_controller_service } else{ - if(request_type == request_type::GET) + if(request_type == streets_snmp_cmd::REQUEST_TYPE::GET) { // Add OID to pdu for get request snmp_add_null_var(pdu, OID, OID_len); } - else if(request_type == request_type::SET) + else if(request_type == streets_snmp_cmd::REQUEST_TYPE::SET) { - if(val.type == snmp_response_obj::response_type::INTEGER){ + if(val.type == streets_snmp_cmd::RESPONSE_TYPE::INTEGER){ snmp_add_var(pdu, OID, OID_len, 'i', (std::to_string(val.val_int)).c_str()); } - else if(val.type == snmp_response_obj::response_type::STRING){ + else if(val.type == streets_snmp_cmd::RESPONSE_TYPE::STRING){ SPDLOG_ERROR("Setting string value is currently not supported"); return false; } @@ -112,7 +112,7 @@ namespace traffic_signal_controller_service SPDLOG_DEBUG("STAT_SUCCESS, received a response"); - if(request_type == request_type::GET){ + if(request_type == streets_snmp_cmd::REQUEST_TYPE::GET){ for(auto vars = response->variables; vars; vars = vars->next_variable){ // Get value of variable depending on ASN.1 type // Variable could be a integer, string, bitstring, ojbid, counter : defined here https://github.com/net-snmp/net-snmp/blob/master/include/net-snmp/types.h @@ -148,13 +148,13 @@ namespace traffic_signal_controller_service } } } - else if(request_type == request_type::SET){ + else if(request_type == streets_snmp_cmd::REQUEST_TYPE::SET){ - if(val.type == snmp_response_obj::response_type::INTEGER){ + if(val.type == streets_snmp_cmd::RESPONSE_TYPE::INTEGER){ SPDLOG_DEBUG("Success in SET for OID: {0} Value: {1}", input_oid ,val.val_int); } - else if(val.type == snmp_response_obj::response_type::STRING){ + else if(val.type == streets_snmp_cmd::RESPONSE_TYPE::STRING){ SPDLOG_DEBUG("Success in SET for OID: {0} Value:", input_oid); for(auto data : val.val_string){ SPDLOG_DEBUG("{0}", data); @@ -178,7 +178,7 @@ namespace traffic_signal_controller_service } - void snmp_client::log_error(const int& status, const request_type& request_type, snmp_pdu *response) const + void snmp_client::log_error(const int& status, const streets_snmp_cmd::REQUEST_TYPE& request_type, snmp_pdu *response) const { if (status == STAT_SUCCESS) @@ -191,10 +191,10 @@ namespace traffic_signal_controller_service SPDLOG_ERROR("Timeout, no response from server"); } else{ - if(request_type == request_type::GET){ + if(request_type == streets_snmp_cmd::REQUEST_TYPE::GET){ SPDLOG_ERROR("Unknown SNMP Error for {0}", "GET"); } - else if(request_type == request_type::SET){ + else if(request_type == streets_snmp_cmd::REQUEST_TYPE::SET){ SPDLOG_ERROR("Unknown SNMP Error for {0}", "SET"); } } diff --git a/tsc_client_service/src/spat_projection_mode.cpp b/tsc_client_service/src/spat_projection_mode.cpp new file mode 100644 index 000000000..a2b9f7011 --- /dev/null +++ b/tsc_client_service/src/spat_projection_mode.cpp @@ -0,0 +1,17 @@ +#include "spat_projection_mode.h" + +namespace traffic_signal_controller_service{ + SPAT_PROJECTION_MODE spat_projection_mode_from_int( const int i ) { + switch (i) + { + case 0: + return SPAT_PROJECTION_MODE::NO_PROJECTION; + case 1: + return SPAT_PROJECTION_MODE::DPP_PROJECTION; + case 2: + return SPAT_PROJECTION_MODE::FIXED_TIMING_PROJECTION; + default: + return SPAT_PROJECTION_MODE::NO_PROJECTION; + } + } +} \ No newline at end of file diff --git a/tsc_client_service/src/tsc_service.cpp b/tsc_client_service/src/tsc_service.cpp index 665ebeba6..5327c2117 100644 --- a/tsc_client_service/src/tsc_service.cpp +++ b/tsc_client_service/src/tsc_service.cpp @@ -3,6 +3,7 @@ namespace traffic_signal_controller_service { std::mutex dpp_mtx; + std::mutex snmp_cmd_queue_mtx; using namespace streets_service; bool tsc_service::initialize() { if (!streets_service::initialize()) { @@ -10,30 +11,6 @@ namespace traffic_signal_controller_service { } try { - // Temporary fix for bug in CarmaClock::wait_for_initialization(). No mechanism to support notifying multiple threads - // of initialization. This fix avoids any threads waiting on initialization. Only required in SIMULATION_MODE=TRUE - // TODO: Remove initialization and fix issue in carma-time-lib (CarmaClock class) - if ( is_simulation_mode() ) { - streets_clock_singleton::update(0); - } - // Intialize spat kafka producer - std::string spat_topic_name = streets_configuration::get_string_config("spat_producer_topic"); - - std::string dpp_consumer_topic = streets_configuration::get_string_config("desired_phase_plan_consumer_topic"); - - if (!spat_producer && !initialize_kafka_producer( spat_topic_name, spat_producer)) { - - SPDLOG_ERROR("Failed to initialize kafka spat_producer!"); - return false; - - } - - if (!desired_phase_plan_consumer && !initialize_kafka_consumer( dpp_consumer_topic, desired_phase_plan_consumer)) { - - SPDLOG_ERROR("Failed to initialize kafka desired_phase_plan_consumer!"); - return false; - - } // Initialize SNMP Client std::string target_ip = streets_configuration::get_string_config("target_ip"); int target_port = streets_configuration::get_int_config("target_port"); @@ -52,25 +29,73 @@ namespace traffic_signal_controller_service { SPDLOG_ERROR("Failed to initialize kafka tsc_config_producer!"); return false; } + //Initialize TSC State - use_desired_phase_plan_update_ = streets_configuration::get_boolean_config("use_desired_phase_plan_update"); if (!initialize_tsc_state(snmp_client_ptr)){ SPDLOG_ERROR("Failed to initialize tsc state"); return false; } tsc_config_state_ptr = tsc_state_ptr->get_tsc_config_state(); + + // Intialize spat kafka producer + std::string spat_topic_name = streets_configuration::get_string_config("spat_producer_topic"); + if ( !spat_producer && !initialize_kafka_producer( spat_topic_name, spat_producer) ) { + + SPDLOG_ERROR("Failed to initialize kafka spat_producer!"); + return false; + + } + + std::string dpp_consumer_topic = streets_configuration::get_string_config("desired_phase_plan_consumer_topic"); + // TODO: Remove if no longer necessary for TM/TSP MMITSS Integration + // use_mmitss_mrp = streets_configuration::get_boolean_config("use_mmitss_mrp"); + // ----------------------------------------------------------------------------- + auto spat_projection_int = streets_configuration::get_int_config("spat_projection_mode"); + spat_proj_mode = spat_projection_mode_from_int(spat_projection_int); + // TODO: Remove if no longer necessary for TM/TSP MMITSS Integration + // Phase control schedule(PCS) consumer topic + // std::string pcs_consumer_topic = streets_configuration::get_string_config("phase_control_schedule_consumer_topic"); + // ----------------------------------------------------------------------------- + + // Initialize DPP consumer and monitor + if ( spat_proj_mode == SPAT_PROJECTION_MODE::DPP_PROJECTION ) { + if (!desired_phase_plan_consumer && !initialize_kafka_consumer( dpp_consumer_topic, desired_phase_plan_consumer)) { + SPDLOG_ERROR("Failed to initialize kafka desired_phase_plan_consumer!"); + return false; + } + monitor_dpp_ptr = std::make_shared( snmp_client_ptr ); + control_tsc_state_sleep_dur_ = streets_configuration::get_int_config("control_tsc_state_sleep_duration"); + // Initialize control_tsc_state ptr + control_tsc_state_ptr_ = std::make_shared(snmp_client_ptr, tsc_state_ptr); + + } + + // TODO: Remove if no longer necessary for TM/TSP MMITSS Integration + // if( SPAT_PROJECTION_MODE::MMITSS_PHASE_SCHEDULE) + // if ( !phase_control_schedule_consumer && !initialize_kafka_consumer(pcs_consumer_topic, phase_control_schedule_consumer)){ + // SPDLOG_ERROR("Failed to initialize kafka phase_control_schedule_consumer!"); + // return false; + // } + // //Initialize phase control schedule + // phase_control_schedule_ptr = std::make_shared(); + // // Initialize control_tsc_state ptr + // control_tsc_state_sleep_dur_ = streets_configuration::get_int_config("control_tsc_state_sleep_duration"); + // control_tsc_state_ptr_ = std::make_shared(snmp_client_ptr, tsc_state_ptr); + // } + // ------------------------------------------------------------------------------------------------------------------ + // Initialize spat_worker std::string socket_ip = streets_configuration::get_string_config("udp_socket_ip"); int socket_port = streets_configuration::get_int_config("udp_socket_port"); int socket_timeout = streets_configuration::get_int_config("socket_timeout"); bool use_msg_timestamp = streets_configuration::get_boolean_config("use_tsc_timestamp"); enable_snmp_cmd_logging_ = streets_configuration::get_boolean_config("enable_snmp_cmd_logging"); - if (!initialize_spat_worker(socket_ip, socket_port, socket_timeout, use_msg_timestamp)) { SPDLOG_ERROR("Failed to initialize SPaT Worker"); return false; } + // Initialize intersection_client if (!initialize_intersection_client()) { SPDLOG_ERROR("Failed to initialize intersection client"); return false; @@ -84,17 +109,10 @@ namespace traffic_signal_controller_service { initialize_spat(intersection_client_ptr->get_intersection_name(), intersection_client_ptr->get_intersection_id(), all_phases); - control_tsc_state_sleep_dur_ = streets_configuration::get_int_config("control_tsc_state_sleep_duration"); - - // Initialize monitor desired phase plan - monitor_dpp_ptr = std::make_shared( snmp_client_ptr ); - - // Initialize control_tsc_state ptr - control_tsc_state_ptr_ = std::make_shared(snmp_client_ptr, tsc_state_ptr); - if (enable_snmp_cmd_logging_) { configure_snmp_cmd_logger(); + configure_veh_ped_call_logger(); } if (is_simulation_mode()) { // Trigger spat broadcasting for EVC on startup. @@ -134,10 +152,10 @@ namespace traffic_signal_controller_service { } bool tsc_service::enable_spat() const{ // Enable SPaT - snmp_response_obj enable_spat; - enable_spat.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj enable_spat; + enable_spat.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; enable_spat.val_int = 2; - if (!snmp_client_ptr->process_snmp_request(ntcip_oids::ENABLE_SPAT_OID, request_type::SET, enable_spat)){ + if (!snmp_client_ptr->process_snmp_request(ntcip_oids::ENABLE_SPAT_OID, streets_snmp_cmd::REQUEST_TYPE::SET, enable_spat)){ SPDLOG_ERROR("Failed to enable SPaT broadcasting on Traffic Signal Controller!"); return false; } @@ -183,24 +201,39 @@ namespace traffic_signal_controller_service { while(spat_worker_ptr && tsc_state_ptr && spat_producer) { try { spat_worker_ptr->receive_spat(spat_ptr); - SPDLOG_DEBUG("Current SPaT : {0} ", spat_ptr->toJson()); - if(!use_desired_phase_plan_update_){ - // throws monitor_states_exception - tsc_state_ptr->add_future_movement_events(spat_ptr); - - }else{ - std::scoped_lock lck{dpp_mtx}; - // throws desired phase plan exception when no previous green information present - monitor_dpp_ptr->update_spat_future_movement_events(spat_ptr, tsc_state_ptr); - } + SPDLOG_DEBUG("Current SPaT : {0} ", spat_ptr->toJson()); + switch (spat_proj_mode) + { + case SPAT_PROJECTION_MODE::FIXED_TIMING_PROJECTION: { + tsc_state_ptr->add_future_movement_events(spat_ptr); + break; + } + case SPAT_PROJECTION_MODE::DPP_PROJECTION : { + std::scoped_lock lck{dpp_mtx}; + // throws desired phase plan exception when no previous green information present + monitor_dpp_ptr->update_spat_future_movement_events(spat_ptr, tsc_state_ptr); + break; + } + default: { + // Poll and log vehicle and pedestrian call information every 10 spat messages or at 10 Hz + if ( count%10 == 0) { + tsc_state_ptr->poll_vehicle_pedestrian_calls(); + if(auto logger = spdlog::get(VEH_PED_CALL_LOGGER_NAME); logger != nullptr ){ + logger->info("{0}, {1}, {2}", streets_clock_singleton::time_in_ms(), + tsc_state_ptr->vector_to_string(tsc_state_ptr->get_vehicle_calls()), + tsc_state_ptr->vector_to_string(tsc_state_ptr->get_pedestrian_calls()) + ); + } + } + } + } if (spat_ptr && spat_producer) { spat_producer->send(spat_ptr->toJson()); // No utility in calculating spat latency in simulation mode if ( !this->is_simulation_mode() ) { log_spat_latency(count, spat_processing_time, spat_ptr->get_intersection().get_epoch_timestamp()) ; } - } - + } } catch( const signal_phase_and_timing::signal_phase_and_timing_exception &e ) { SPDLOG_ERROR("Encountered exception, spat not published : \n {0}", e.what()); @@ -211,7 +244,7 @@ namespace traffic_signal_controller_service { catch(const traffic_signal_controller_service::monitor_states_exception &e){ SPDLOG_ERROR("Could not update movement events, spat not published. Encountered exception : \n {0}", e.what()); } - } + } SPDLOG_WARN("Stopping produce_spat_json! Are pointers null: spat_worker {0}, spat_producer {1}, tsc_state {2}", spat_worker_ptr == nullptr, spat_ptr == nullptr, tsc_state_ptr == nullptr); } @@ -249,6 +282,7 @@ namespace traffic_signal_controller_service { // update command queue if(monitor_dpp_ptr->get_desired_phase_plan_ptr()){ // Send desired phase plan to control_tsc_state + std::scoped_lock snmp_cmd_lck(snmp_cmd_queue_mtx); control_tsc_state_ptr_->update_tsc_control_queue(monitor_dpp_ptr->get_desired_phase_plan_ptr(), tsc_set_command_queue_); } @@ -256,82 +290,144 @@ namespace traffic_signal_controller_service { } } + // TODO: Remove if no longer necessary for TM/TSP MMITSS Integration + // void tsc_service::consume_phase_control_schedule(){ + // phase_control_schedule_consumer->subscribe(); + // while (phase_control_schedule_consumer->is_running()) + // { + // const std::string payload = phase_control_schedule_consumer->consume(1000); + // if (payload.length() != 0) + // { + // SPDLOG_DEBUG("Consumed: {0}", payload); + // try { + // //Update phase control schedule with the latest incoming schedule + // phase_control_schedule_ptr->fromJson(payload); + // //Update command queue + // std::scoped_lock snmp_cmd_lck(snmp_cmd_queue_mtx); + // control_tsc_state_ptr_->update_tsc_control_queue(phase_control_schedule_ptr, tsc_set_command_queue_); + // } catch(streets_phase_control_schedule::streets_phase_control_schedule_exception &ex){ + // SPDLOG_DEBUG("Failed to consume phase control schedule commands: {0}", ex.what()); + // } + // } + // } + // } + // -------------------------------------------------------------------------------------------- void tsc_service::control_tsc_phases() { - try{ - SPDLOG_INFO("Starting TSC Control Phases"); + SPDLOG_INFO("Starting TSC Control Phases"); + try + { while(true) { SPDLOG_INFO("Iterate TSC Control Phases for time {0}", streets_clock_singleton::time_in_ms()); - set_tsc_hold_and_omit(); + set_tsc_hold_and_omit_forceoff_call(); streets_clock_singleton::sleep_for(control_tsc_state_sleep_dur_); } } catch(const control_tsc_state_exception &e){ SPDLOG_ERROR("Encountered exception : \n {0}", e.what()); - SPDLOG_ERROR("Removing front command from queue : {0}", tsc_set_command_queue_.front().get_cmd_info()); - tsc_set_command_queue_.pop(); - + if(!tsc_set_command_queue_.empty()) + { + SPDLOG_ERROR("Removing front command from queue : {0}", tsc_set_command_queue_.front().get_cmd_info()); + std::scoped_lock snmp_cmd_lck(snmp_cmd_queue_mtx); + tsc_set_command_queue_.pop(); + } } - } - void tsc_service::set_tsc_hold_and_omit() + void tsc_service::set_tsc_hold_and_omit_forceoff_call() { - - while(!tsc_set_command_queue_.empty()) + if(control_tsc_state_ptr_ && !tsc_set_command_queue_.empty()) { - SPDLOG_DEBUG("Checking if front command {0} is expired", tsc_set_command_queue_.front().get_cmd_info()); + //Clear all phase controls from the traffic signal controller + SPDLOG_DEBUG("Clear all phase controls from TSC!"); + control_tsc_state_ptr_->run_clear_all_snmp_commands(); + } + while(!tsc_set_command_queue_.empty()) + { + auto current_command = tsc_set_command_queue_.front(); + SPDLOG_DEBUG("Checking if front command {0} is expired", current_command.get_cmd_info()); //Check if event is expired - long duration = tsc_set_command_queue_.front().start_time_ - streets_clock_singleton::time_in_ms(); + long duration = current_command.start_time_ - streets_clock_singleton::time_in_ms(); if(duration < 0){ throw control_tsc_state_exception("SNMP set command is expired! Start time was " - + std::to_string(tsc_set_command_queue_.front().start_time_) + " and current time is " + + std::to_string(current_command.start_time_) + " and current time is " + std::to_string(streets_clock_singleton::time_in_ms() ) + "."); - } + } SPDLOG_DEBUG("Sleeping for {0}ms.", duration); streets_clock_singleton::sleep_for(duration); + + //After sleep duration, check the snmp queue again. If empty, stop the current command execution + if(tsc_set_command_queue_.empty()) + { + SPDLOG_DEBUG("SNMP command queue is empty, stop the current command execution: {0}", current_command.get_cmd_info()); + break; + } + //Check if queue commands has been updated + else if (tsc_set_command_queue_.front().get_cmd_info() != current_command.get_cmd_info()) + { + SPDLOG_DEBUG("SNMP command queue is updated with new schedule, stop the current command execution: {0}", current_command.get_cmd_info()); + break; + } - if(!(tsc_set_command_queue_.front()).run()) + if(!control_tsc_state_ptr_) { - throw control_tsc_state_exception("Could not set state for movement group in desired phase plan"); + throw control_tsc_state_exception("Control TSC state is not initialized."); } - // Log command info sent - SPDLOG_INFO(tsc_set_command_queue_.front().get_cmd_info()); - - if ( enable_snmp_cmd_logging_ ){ - if(auto logger = spdlog::get("snmp_cmd_logger"); logger != nullptr ){ - logger->info( tsc_set_command_queue_.front().get_cmd_info()); + //Checking all the commands from the queue. If there are any commands executed at the same time as the current_command, and execute these commands now. + while (!tsc_set_command_queue_.empty()) + { + auto command_to_execute = tsc_set_command_queue_.front(); + if(command_to_execute.start_time_ == current_command.start_time_) + { + if(control_tsc_state_ptr_ && !control_tsc_state_ptr_->run_snmp_cmd_set_request(command_to_execute)) + { + throw control_tsc_state_exception("Could not set state for movement group in desired phase plan/phase control schedule."); + } + // Log command info sent + SPDLOG_INFO(command_to_execute.get_cmd_info()); + if ( enable_snmp_cmd_logging_ ){ + if(auto logger = spdlog::get("snmp_cmd_logger"); logger != nullptr ){ + logger->info( command_to_execute.get_cmd_info()); + } + } + //SNMP command Queue has no update nor has been cleared, pop the existing front command. + std::scoped_lock snmp_cmd_lck(snmp_cmd_queue_mtx); + tsc_set_command_queue_.pop(); } + else + { + break; + } } - - tsc_set_command_queue_.pop(); } } void tsc_service::configure_snmp_cmd_logger() const { - try{ - auto snmp_cmd_logger = spdlog::daily_logger_mt( - "snmp_cmd_logger", // logger name - streets_configuration::get_string_config("snmp_cmd_log_path")+ - streets_configuration::get_string_config("snmp_cmd_log_filename") +".log", // log file name and path - 23, // hours to rotate - 59 // minutes to rotate - ); - // Only log log statement content - snmp_cmd_logger->set_pattern("[%H:%M:%S:%e ] %v"); - snmp_cmd_logger->set_level(spdlog::level::info); - snmp_cmd_logger->flush_on(spdlog::level::info); + try { + create_daily_logger(SNMP_COMMAND_LOGGER_NAME, ".log", "[%H:%M:%S:%e ] %v", spdlog::level::info ); } catch (const spdlog::spdlog_ex& ex) { - spdlog::error( "Log initialization failed: {0}!",ex.what()); + SPDLOG_ERROR( "SNMP Command Logger initialization failed: {0}!",ex.what()); } } + void tsc_service::configure_veh_ped_call_logger() const + { + try { + auto veh_ped_call_logger = create_daily_logger(VEH_PED_CALL_LOGGER_NAME, ".csv", "%v", spdlog::level::info ); + } + catch (const spdlog::spdlog_ex& ex) + { + SPDLOG_ERROR( "Vehicle Pedestrian Call Logger initialization failed: {0}!",ex.what()); + } + // TODO: Figure out how to termine if a new file was created or an existing file appended and only write column headers on new files + // veh_ped_call_logger->info("Timestamp (ms), Vehicle Calls (Signal Group ID), Pedestrian Calls (Signal Group ID)"); + } void tsc_service::log_spat_latency(int &count, uint64_t &spat_processing_time, uint64_t spat_time_stamp) const { // Calculate and average spat processing time over 20 messages sent if (count <= 20 ) { @@ -356,15 +452,31 @@ namespace traffic_signal_controller_service { std::thread spat_t(&tsc_service::produce_spat_json, this); - std::thread desired_phase_plan_t(&tsc_service::consume_desired_phase_plan, this); - - std::thread control_phases_t(&tsc_service::control_tsc_phases, this); + if ( spat_proj_mode == SPAT_PROJECTION_MODE::DPP_PROJECTION ) { + std::thread desired_phase_plan_t(&tsc_service::consume_desired_phase_plan, this); + SPDLOG_DEBUG("Thread joined to consume desired phase plan generated by carma-streets SO service."); + desired_phase_plan_t.join(); + // Run tsc control phases + std::thread control_phases_t(&tsc_service::control_tsc_phases, this); + control_phases_t.join(); + } + // TODO: Remove if no longer necessary for TM/TSP MMITSS Integration + // An indicator to control whether launching a thread to consume carma-streets internal desired phase plan generated by singal optimization (SO) service + // or a thread to consume MMITSS external phase control schedule generated by MRP (https://github.com/mmitss/mmitss-az) + // if ( spat_proj_mode == SPAT_PROJECTION_MODE::MMITSS_PHASE_SCHEDULE ) { + // std::thread consume_phase_control_schedule_t(&tsc_service::consume_phase_control_schedule, this); + // SPDLOG_DEBUG("Thread joined to consume phase control schedule generated by MRP."); + // consume_phase_control_schedule_t.join(); + // // Run tsc control phases + // std::thread control_phases_t(&tsc_service::control_tsc_phases, this); + // control_phases_t.join(); + // } + // --------------------------------------------------------------------------------------------------------------- // Run threads as joint so that they dont overlap execution tsc_config_thread.join(); spat_t.join(); - desired_phase_plan_t.join(); - control_phases_t.join(); + } @@ -385,5 +497,10 @@ namespace traffic_signal_controller_service { SPDLOG_WARN("Stopping desired phase plan consumer!"); desired_phase_plan_consumer->stop(); } + + if (phase_control_schedule_consumer) { + SPDLOG_WARN("Stopping phase control consumer!"); + phase_control_schedule_consumer->stop(); + } } } \ No newline at end of file diff --git a/tsc_client_service/test/test_control_tsc_state.cpp b/tsc_client_service/test/test_control_tsc_state.cpp index 8d3984d56..c187e2ca9 100644 --- a/tsc_client_service/test/test_control_tsc_state.cpp +++ b/tsc_client_service/test/test_control_tsc_state.cpp @@ -26,25 +26,25 @@ namespace traffic_signal_controller_service int phase_num = 0; const std::string&input_oid = ""; - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; // Test get max channels - snmp_response_obj max_channels_in_tsc; + streets_snmp_cmd::snmp_response_obj max_channels_in_tsc; max_channels_in_tsc.val_int =8; - max_channels_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_channels_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::MAX_CHANNELS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_channels_in_tsc), Return(true))); - snmp_response_obj max_rings_in_tsc; + streets_snmp_cmd::snmp_response_obj max_rings_in_tsc; max_rings_in_tsc.val_int = 4; - max_rings_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_rings_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::MAX_RINGS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_rings_in_tsc), Return(true))); // Define Sequence Data - snmp_response_obj seq_data; + streets_snmp_cmd::snmp_response_obj seq_data; seq_data.val_string = {char(1),char(2), char(3),char(4)}; std::string seq_data_ring1_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(1); @@ -73,7 +73,7 @@ namespace traffic_signal_controller_service for(int i = 1; i <= max_channels_in_tsc.val_int; ++i){ // Define Control Type - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_client, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -81,7 +81,7 @@ namespace traffic_signal_controller_service testing::Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = i; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_client, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -92,7 +92,7 @@ namespace traffic_signal_controller_service // Define get min green std::string min_green_oid = ntcip_oids::MINIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj min_green; + streets_snmp_cmd::snmp_response_obj min_green; min_green.val_int = 20; EXPECT_CALL(*mock_client, process_snmp_request(min_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(min_green), @@ -101,7 +101,7 @@ namespace traffic_signal_controller_service // Define get max green std::string max_green_oid = ntcip_oids::MAXIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj max_green; + streets_snmp_cmd::snmp_response_obj max_green; max_green.val_int = 30; EXPECT_CALL(*mock_client, process_snmp_request(max_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_green), @@ -110,7 +110,7 @@ namespace traffic_signal_controller_service // Define get yellow Duration std::string yellow_oid = ntcip_oids::YELLOW_CHANGE_PARAMETER + "." + std::to_string(i); - snmp_response_obj yellow_duration; + streets_snmp_cmd::snmp_response_obj yellow_duration; yellow_duration.val_int = 40; EXPECT_CALL(*mock_client, process_snmp_request(yellow_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(yellow_duration), @@ -120,7 +120,7 @@ namespace traffic_signal_controller_service // Define red clearance std::string red_clearance_oid = ntcip_oids::RED_CLEAR_PARAMETER + "." + std::to_string(i); - snmp_response_obj red_clearance_duration; + streets_snmp_cmd::snmp_response_obj red_clearance_duration; red_clearance_duration.val_int = 10; EXPECT_CALL(*mock_client, process_snmp_request(red_clearance_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(red_clearance_duration), @@ -128,7 +128,7 @@ namespace traffic_signal_controller_service //Define get concurrent phases std::string concurrent_phase_oid = ntcip_oids::PHASE_CONCURRENCY + "." + std::to_string(i); - snmp_response_obj concurrent_phase_resp; + streets_snmp_cmd::snmp_response_obj concurrent_phase_resp; if ( i == 1 || i == 2) { concurrent_phase_resp.val_string = {char(5), char(6)}; } @@ -150,11 +150,11 @@ namespace traffic_signal_controller_service } // Define Control Type - snmp_response_obj hold_control; + streets_snmp_cmd::snmp_response_obj hold_control; hold_control.val_int = 255; - hold_control.type = snmp_response_obj::response_type::INTEGER; + hold_control.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; - EXPECT_CALL(*mock_client, process_snmp_request(_, request_type::SET , _) ) + EXPECT_CALL(*mock_client, process_snmp_request(_, streets_snmp_cmd::REQUEST_TYPE::SET , _) ) .WillRepeatedly(testing::DoAll(testing::Return(true))); @@ -190,25 +190,67 @@ namespace traffic_signal_controller_service // Test update queue - std::queue control_commands_queue; + std::queue control_commands_queue; EXPECT_NO_THROW(worker.update_tsc_control_queue(desired_phase_plan_ptr,control_commands_queue)); EXPECT_NO_THROW(worker.update_tsc_control_queue(desired_phase_plan_ptr_2,control_commands_queue)); desired_phase_plan_ptr->desired_phase_plan.back().signal_groups = {1,6}; EXPECT_THROW(worker.update_tsc_control_queue(desired_phase_plan_ptr,control_commands_queue), control_tsc_state_exception); - // Test snmp_cmd_struct - snmp_cmd_struct test_control_obj(mock_client, event1.start_time,snmp_cmd_struct::control_type::Hold, 0); - EXPECT_TRUE(test_control_obj.run()); + // Test streets_snmp_cmd::snmp_cmd_struct + streets_snmp_cmd::snmp_cmd_struct test_control_obj(event1.start_time,streets_snmp_cmd::PHASE_CONTROL_TYPE::HOLD_VEH_PHASES, 0); + EXPECT_TRUE(worker.run_snmp_cmd_set_request(test_control_obj)); - snmp_cmd_struct test_control_obj_2(mock_client, event1.start_time,snmp_cmd_struct::control_type::Omit, 0); - EXPECT_TRUE(test_control_obj_2.run()); + streets_snmp_cmd::snmp_cmd_struct test_control_obj_2( event1.start_time,streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_VEH_PHASES, 0); + EXPECT_TRUE(worker.run_snmp_cmd_set_request(test_control_obj_2)); + + streets_snmp_cmd::snmp_cmd_struct test_control_obj_3( event1.start_time,streets_snmp_cmd::PHASE_CONTROL_TYPE::OMIT_PED_PHASES, 0); + EXPECT_TRUE(worker.run_snmp_cmd_set_request(test_control_obj_3)); + + streets_snmp_cmd::snmp_cmd_struct test_control_obj_4( event1.start_time,streets_snmp_cmd::PHASE_CONTROL_TYPE::FORCEOFF_PHASES, 0); + EXPECT_TRUE(worker.run_snmp_cmd_set_request(test_control_obj_4)); + + streets_snmp_cmd::snmp_cmd_struct test_control_obj_5( event1.start_time,streets_snmp_cmd::PHASE_CONTROL_TYPE::CALL_PED_PHASES, 0); + EXPECT_TRUE(worker.run_snmp_cmd_set_request(test_control_obj_5)); + + streets_snmp_cmd::snmp_cmd_struct test_control_obj_6( event1.start_time,streets_snmp_cmd::PHASE_CONTROL_TYPE::CALL_VEH_PHASES, 0); + EXPECT_TRUE(worker.run_snmp_cmd_set_request(test_control_obj_6)); + EXPECT_NO_THROW(worker.run_clear_all_snmp_commands()); // Test empty desired phase plan streets_desired_phase_plan::streets_desired_phase_plan desired_phase_plan_2; auto dpp_ptr_2 = std::make_shared(desired_phase_plan_2); EXPECT_NO_THROW(worker.update_tsc_control_queue(dpp_ptr_2,control_commands_queue)); + auto pcs_ptr = std::make_shared(); + std::string input_schedule_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"hold\"},{\"commandEndTime\":24,\"commandPhase\":4,\"commandStartTime\":5,\"commandType\":\"hold\"},{\"commandEndTime\":44,\"commandPhase\":2,\"commandStartTime\":29,\"commandType\":\"hold\"},{\"commandEndTime\":64,\"commandPhase\":4,\"commandStartTime\":49,\"commandType\":\"hold\"},{\"commandEndTime\":12,\"commandPhase\":2,\"commandStartTime\":11,\"commandType\":\"forceoff\"},{\"commandEndTime\":69,\"commandPhase\":4,\"commandStartTime\":68,\"commandType\":\"forceoff\"},{\"commandEndTime\":109,\"commandPhase\":2,\"commandStartTime\":108,\"commandType\":\"forceoff\"},{\"commandEndTime\":129,\"commandPhase\":4,\"commandStartTime\":128,\"commandType\":\"forceoff\"},{\"commandEndTime\":23,\"commandPhase\":4,\"commandStartTime\":0,\"commandType\":\"call_veh\"},{\"commandEndTime\":133,\"commandPhase\":1,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":133,\"commandPhase\":3,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":0,\"commandPhase\":6,\"commandStartTime\":0,\"commandType\":\"hold\"},{\"commandEndTime\":24,\"commandPhase\":7,\"commandStartTime\":5,\"commandType\":\"hold\"},{\"commandEndTime\":44,\"commandPhase\":6,\"commandStartTime\":29,\"commandType\":\"hold\"},{\"commandEndTime\":64,\"commandPhase\":7,\"commandStartTime\":49,\"commandType\":\"hold\"},{\"commandEndTime\":12,\"commandPhase\":6,\"commandStartTime\":11,\"commandType\":\"forceoff\"},{\"commandEndTime\":69,\"commandPhase\":7,\"commandStartTime\":68,\"commandType\":\"forceoff\"},{\"commandEndTime\":109,\"commandPhase\":6,\"commandStartTime\":108,\"commandType\":\"forceoff\"},{\"commandEndTime\":129,\"commandPhase\":7,\"commandStartTime\":128,\"commandType\":\"forceoff\"},{\"commandEndTime\":23,\"commandPhase\":7,\"commandStartTime\":0,\"commandType\":\"call_veh\"},{\"commandEndTime\":133,\"commandPhase\":5,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":133,\"commandPhase\":8,\"commandStartTime\":0,\"commandType\":\"omit_veh\"}]}"; + pcs_ptr->fromJson(input_schedule_str); + worker.update_tsc_control_queue(pcs_ptr, control_commands_queue); } + + TEST(traffic_signal_controller_service, update_tsc_control_queue_by_phase_control_schedule) + { + // Define Worker + auto mock_client = std::make_shared(); + auto _tsc_state = std::make_shared(mock_client); + _tsc_state->initialize(); + traffic_signal_controller_service::control_tsc_state worker(mock_client, _tsc_state); + + //Update queue with new schedule that has commands + auto pcs_ptr = std::make_shared(); + std::string input_schedule_str = "{\"MsgType\":\"Schedule\",\"Schedule\":[{\"commandEndTime\":0,\"commandPhase\":2,\"commandStartTime\":0,\"commandType\":\"hold\"},{\"commandEndTime\":24,\"commandPhase\":4,\"commandStartTime\":5,\"commandType\":\"hold\"},{\"commandEndTime\":44,\"commandPhase\":2,\"commandStartTime\":29,\"commandType\":\"hold\"},{\"commandEndTime\":64,\"commandPhase\":4,\"commandStartTime\":49,\"commandType\":\"hold\"},{\"commandEndTime\":12,\"commandPhase\":2,\"commandStartTime\":11,\"commandType\":\"forceoff\"},{\"commandEndTime\":69,\"commandPhase\":4,\"commandStartTime\":68,\"commandType\":\"forceoff\"},{\"commandEndTime\":109,\"commandPhase\":2,\"commandStartTime\":108,\"commandType\":\"forceoff\"},{\"commandEndTime\":129,\"commandPhase\":4,\"commandStartTime\":128,\"commandType\":\"forceoff\"},{\"commandEndTime\":23,\"commandPhase\":4,\"commandStartTime\":0,\"commandType\":\"call_veh\"},{\"commandEndTime\":133,\"commandPhase\":1,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":133,\"commandPhase\":3,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":0,\"commandPhase\":6,\"commandStartTime\":0,\"commandType\":\"hold\"},{\"commandEndTime\":24,\"commandPhase\":7,\"commandStartTime\":5,\"commandType\":\"hold\"},{\"commandEndTime\":44,\"commandPhase\":6,\"commandStartTime\":29,\"commandType\":\"hold\"},{\"commandEndTime\":64,\"commandPhase\":7,\"commandStartTime\":49,\"commandType\":\"hold\"},{\"commandEndTime\":12,\"commandPhase\":6,\"commandStartTime\":11,\"commandType\":\"forceoff\"},{\"commandEndTime\":69,\"commandPhase\":7,\"commandStartTime\":68,\"commandType\":\"forceoff\"},{\"commandEndTime\":109,\"commandPhase\":6,\"commandStartTime\":108,\"commandType\":\"forceoff\"},{\"commandEndTime\":129,\"commandPhase\":7,\"commandStartTime\":128,\"commandType\":\"forceoff\"},{\"commandEndTime\":23,\"commandPhase\":7,\"commandStartTime\":0,\"commandType\":\"call_veh\"},{\"commandEndTime\":133,\"commandPhase\":5,\"commandStartTime\":0,\"commandType\":\"omit_veh\"},{\"commandEndTime\":133,\"commandPhase\":8,\"commandStartTime\":0,\"commandType\":\"omit_veh\"}]}"; + pcs_ptr->fromJson(input_schedule_str); + + // Test update queue with clear schedule + std::queue control_commands_queue; + worker.update_tsc_control_queue(pcs_ptr, control_commands_queue); + ASSERT_EQ(19, control_commands_queue.size()); + input_schedule_str = "{\"MsgType\":\"Schedule\",\"Schedule\":\"Clear\"}"; + pcs_ptr->fromJson(input_schedule_str); + worker.update_tsc_control_queue(pcs_ptr, control_commands_queue); + std::shared_ptr pcs_ptr_null; + worker.update_tsc_control_queue(pcs_ptr_null, control_commands_queue); + ASSERT_EQ(0, control_commands_queue.size()); + } } \ No newline at end of file diff --git a/tsc_client_service/test/test_monitor_desired_phase_plan.cpp b/tsc_client_service/test/test_monitor_desired_phase_plan.cpp index e7d924753..ac8444218 100644 --- a/tsc_client_service/test/test_monitor_desired_phase_plan.cpp +++ b/tsc_client_service/test/test_monitor_desired_phase_plan.cpp @@ -241,24 +241,24 @@ namespace traffic_signal_controller_service void mock_tsc_ntcip() { // gmock SNMP response --------------------------------------------------------------------------------------------------------------------- // Test get max channels - snmp_response_obj max_channels_in_tsc; + streets_snmp_cmd::snmp_response_obj max_channels_in_tsc; max_channels_in_tsc.val_int = 16; - max_channels_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_channels_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; EXPECT_CALL( *mock_snmp, process_snmp_request(ntcip_oids::MAX_CHANNELS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_channels_in_tsc), Return(true))); - snmp_response_obj max_rings_in_tsc; + streets_snmp_cmd::snmp_response_obj max_rings_in_tsc; max_rings_in_tsc.val_int = 4; - max_rings_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_rings_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; EXPECT_CALL( *mock_snmp, process_snmp_request(ntcip_oids::MAX_RINGS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_rings_in_tsc), Return(true))); // Define Sequence Data - snmp_response_obj seq_data; + streets_snmp_cmd::snmp_response_obj seq_data; seq_data.val_string = {char(1),char(2), char(3), char(4)}; std::string seq_data_ring1_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(1); @@ -293,7 +293,7 @@ namespace traffic_signal_controller_service // switch(i) { case 1: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -301,7 +301,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 1; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -310,7 +310,7 @@ namespace traffic_signal_controller_service break; } case 2: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -318,7 +318,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 2; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -327,7 +327,7 @@ namespace traffic_signal_controller_service break; } case 3: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -335,7 +335,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 3; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -344,7 +344,7 @@ namespace traffic_signal_controller_service break; } case 4: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -352,7 +352,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 4; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -361,7 +361,7 @@ namespace traffic_signal_controller_service break; } case 5: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -369,7 +369,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 5; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -378,7 +378,7 @@ namespace traffic_signal_controller_service break; } case 6: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -386,7 +386,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 6; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -395,7 +395,7 @@ namespace traffic_signal_controller_service break; } case 7: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -403,7 +403,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 7; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -412,7 +412,7 @@ namespace traffic_signal_controller_service break; } case 8: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -420,7 +420,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 8; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -429,7 +429,7 @@ namespace traffic_signal_controller_service break; } default: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 0; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -439,7 +439,7 @@ namespace traffic_signal_controller_service // Define Control Source // Note this OID is not actually called for any non vehicle/pedestrian phases which is why the Times() assertion // is not included - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 0; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -454,7 +454,7 @@ namespace traffic_signal_controller_service // Define get min green std::string min_green_oid = ntcip_oids::MINIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj min_green; + streets_snmp_cmd::snmp_response_obj min_green; min_green.val_int = 5; EXPECT_CALL(*mock_snmp, process_snmp_request(min_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(min_green), @@ -463,7 +463,7 @@ namespace traffic_signal_controller_service // Define get max green std::string max_green_oid = ntcip_oids::MAXIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj max_green; + streets_snmp_cmd::snmp_response_obj max_green; max_green.val_int = 300; EXPECT_CALL(*mock_snmp, process_snmp_request(max_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_green), @@ -472,7 +472,7 @@ namespace traffic_signal_controller_service // Define get yellow Duration std::string yellow_oid = ntcip_oids::YELLOW_CHANGE_PARAMETER + "." + std::to_string(i); - snmp_response_obj yellow_duration; + streets_snmp_cmd::snmp_response_obj yellow_duration; yellow_duration.val_int = 10; EXPECT_CALL(*mock_snmp, process_snmp_request(yellow_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(yellow_duration), @@ -482,7 +482,7 @@ namespace traffic_signal_controller_service // Define red clearance std::string red_clearance_oid = ntcip_oids::RED_CLEAR_PARAMETER + "." + std::to_string(i); - snmp_response_obj red_clearance_duration; + streets_snmp_cmd::snmp_response_obj red_clearance_duration; red_clearance_duration.val_int = 15; EXPECT_CALL(*mock_snmp, process_snmp_request(red_clearance_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(red_clearance_duration), @@ -491,7 +491,7 @@ namespace traffic_signal_controller_service //Define get concurrent phases std::string concurrent_phase_oid = ntcip_oids::PHASE_CONCURRENCY + "." + std::to_string(i); - snmp_response_obj concurrent_phase_resp; + streets_snmp_cmd::snmp_response_obj concurrent_phase_resp; if ( i == 1 || i == 2) { concurrent_phase_resp.val_string = {char(5), char(6)}; } @@ -1534,11 +1534,11 @@ namespace traffic_signal_controller_service last_green_served.push_back(five); monitor_dpp_ptr->last_green_served = last_green_served ; // mock next phase NTCIP OID - snmp_response_obj next_phase; + streets_snmp_cmd::snmp_response_obj next_phase; next_phase.val_int = 68; // Binary 01000100 -> phase 3 and phase 7 next green std::string next_phase_oid = ntcip_oids::PHASE_STATUS_GROUP_PHASE_NEXT; - EXPECT_CALL(*mock_snmp, process_snmp_request(next_phase_oid, request_type::GET , _) ).Times(1). + EXPECT_CALL(*mock_snmp, process_snmp_request(next_phase_oid, streets_snmp_cmd::REQUEST_TYPE::GET , _) ).Times(1). WillRepeatedly( testing::DoAll( SetArgReferee<2>(next_phase), @@ -1621,11 +1621,11 @@ namespace traffic_signal_controller_service monitor_dpp_ptr->last_green_served = last_green_served ; // mock next phase NTCIP OID - snmp_response_obj next_phase; + streets_snmp_cmd::snmp_response_obj next_phase; next_phase.val_int = 68; // Binary 01000100 -> phase 3 and phase 7 next green std::string next_phase_oid = ntcip_oids::PHASE_STATUS_GROUP_PHASE_NEXT; - EXPECT_CALL(*mock_snmp, process_snmp_request(next_phase_oid, request_type::GET , _) ).Times(1). + EXPECT_CALL(*mock_snmp, process_snmp_request(next_phase_oid, streets_snmp_cmd::REQUEST_TYPE::GET , _) ).Times(1). WillRepeatedly( testing::DoAll( SetArgReferee<2>(next_phase), @@ -1729,11 +1729,11 @@ namespace traffic_signal_controller_service two.state_time_speed.push_back(green); // mock next phase NTCIP OID - snmp_response_obj next_phase; + streets_snmp_cmd::snmp_response_obj next_phase; next_phase.val_int = 68; // Binary 01000100 -> phase 3 and phase 7 next green std::string next_phase_oid = ntcip_oids::PHASE_STATUS_GROUP_PHASE_NEXT; - EXPECT_CALL(*mock_snmp, process_snmp_request(next_phase_oid, request_type::GET , _) ).Times(1). + EXPECT_CALL(*mock_snmp, process_snmp_request(next_phase_oid, streets_snmp_cmd::REQUEST_TYPE::GET , _) ).Times(1). WillRepeatedly( testing::DoAll( SetArgReferee<2>(next_phase), diff --git a/tsc_client_service/test/test_monitor_state.cpp b/tsc_client_service/test/test_monitor_state.cpp index 818c9f79e..98fda40bf 100644 --- a/tsc_client_service/test/test_monitor_state.cpp +++ b/tsc_client_service/test/test_monitor_state.cpp @@ -14,6 +14,162 @@ using testing::SetArgReferee; namespace traffic_signal_controller_service { + TEST(test_monitor_state, poll_vehicle_pedestrian_calls){ + auto mock_client = std::make_shared(); + const std::string&input_oid = ""; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; + + // Test get max channels + streets_snmp_cmd::snmp_response_obj vehicle_calls; + vehicle_calls.val_int = 15; + vehicle_calls.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + + EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::PHASE_STATUS_GROUP_VEH_CALLS + ".1", request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(vehicle_calls), + Return(true))); + vehicle_calls.val_int = 0; + + EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::PHASE_STATUS_GROUP_VEH_CALLS + ".2", request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(vehicle_calls), + Return(true))); + vehicle_calls.val_int = 0; + + EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::PHASE_STATUS_GROUP_PED_CALLS + ".1", request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(vehicle_calls), + Return(true))); + vehicle_calls.val_int = 15; + + EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::PHASE_STATUS_GROUP_PED_CALLS + ".2", request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(vehicle_calls), + Return(true))); + + // Test get max channels + streets_snmp_cmd::snmp_response_obj max_channels_in_tsc; + max_channels_in_tsc.val_int = 12; + max_channels_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + + EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::MAX_CHANNELS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(max_channels_in_tsc), + Return(true))); + + streets_snmp_cmd::snmp_response_obj max_rings_in_tsc; + max_rings_in_tsc.val_int = 4; + max_rings_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; + EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::MAX_RINGS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(max_rings_in_tsc), + Return(true))); + // Define Sequence Data + streets_snmp_cmd::snmp_response_obj seq_data; + seq_data.val_string = {char(1),char(2),char(5), char(6)}; + std::string seq_data_ring1_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(1); + + EXPECT_CALL(*mock_client, process_snmp_request(seq_data_ring1_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(seq_data), + Return(true))); + + seq_data.val_string = {char(3),char(4), char(7), char(8)}; + std::string seq_data_ring2_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(2); + EXPECT_CALL(*mock_client, process_snmp_request(seq_data_ring2_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(seq_data), + Return(true))); + + seq_data.val_string = {char(9),char(10)}; + std::string seq_data_ring3_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(3); + EXPECT_CALL(*mock_client, process_snmp_request(seq_data_ring3_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(seq_data), + Return(true))); + + seq_data.val_string = {char(11),char(12)}; + + std::string seq_data_ring4_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(4); + EXPECT_CALL(*mock_client, process_snmp_request(seq_data_ring4_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(seq_data), + Return(true))); + + //get_vehicle_phase channel + for(int i = 1; i <= max_channels_in_tsc.val_int; ++i){ + + // Define Control Type + streets_snmp_cmd::snmp_response_obj channel_control_resp; + if ( i > 8) + channel_control_resp.val_int = 3; + else + channel_control_resp.val_int = 2; + + std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); + EXPECT_CALL(*mock_client, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(channel_control_resp), + testing::Return(true))); + + // Define Control Source + streets_snmp_cmd::snmp_response_obj control_source_resp; + control_source_resp.val_int = i; + std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); + EXPECT_CALL(*mock_client, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(control_source_resp), + Return(true))); + + + if (i > 8) { + continue; + } + // Define get min green + std::string min_green_oid = ntcip_oids::MINIMUM_GREEN + "." + std::to_string(i); + streets_snmp_cmd::snmp_response_obj min_green; + min_green.val_int = 20; + EXPECT_CALL(*mock_client, process_snmp_request(min_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(min_green), + Return(true))); + + + // Define get max green + std::string max_green_oid = ntcip_oids::MAXIMUM_GREEN + "." + std::to_string(i); + streets_snmp_cmd::snmp_response_obj max_green; + max_green.val_int = 30; + EXPECT_CALL(*mock_client, process_snmp_request(max_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(max_green), + Return(true))); + + + // Define get yellow Duration + std::string yellow_oid = ntcip_oids::YELLOW_CHANGE_PARAMETER + "." + std::to_string(i); + streets_snmp_cmd::snmp_response_obj yellow_duration; + yellow_duration.val_int = 40; + EXPECT_CALL(*mock_client, process_snmp_request(yellow_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(yellow_duration), + Return(true))); + + + + // Define red clearance + std::string red_clearance_oid = ntcip_oids::RED_CLEAR_PARAMETER + "." + std::to_string(i); + streets_snmp_cmd::snmp_response_obj red_clearance_duration; + red_clearance_duration.val_int = 10; + EXPECT_CALL(*mock_client, process_snmp_request(red_clearance_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(red_clearance_duration), + Return(true))); + + //Define get concurrent phases + std::string concurrent_phase_oid = ntcip_oids::PHASE_CONCURRENCY + "." + std::to_string(i); + streets_snmp_cmd::snmp_response_obj concurrent_phase_resp; + if ( i == 1 || i == 2) { + concurrent_phase_resp.val_string = {char(1), char(2)}; + } + else if (i == 3|| i == 4) { + concurrent_phase_resp.val_string = {char(3), char(4)}; + + } + EXPECT_CALL(*mock_client, process_snmp_request(concurrent_phase_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( + SetArgReferee<2>(concurrent_phase_resp), + testing::Return(true))); + } + traffic_signal_controller_service::tsc_state worker(mock_client); + worker.initialize(); + worker.poll_vehicle_pedestrian_calls(); + + ASSERT_EQ(4, worker.get_pedestrian_calls().size()); + ASSERT_EQ(4 , worker.get_vehicle_calls().size()); + } TEST(test_monitor_state, test_get_tsc_state) { @@ -21,26 +177,26 @@ namespace traffic_signal_controller_service int phase_num = 0; const std::string&input_oid = ""; - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; // Test get max channels - snmp_response_obj max_channels_in_tsc; + streets_snmp_cmd::snmp_response_obj max_channels_in_tsc; max_channels_in_tsc.val_int = 4; - max_channels_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_channels_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::MAX_CHANNELS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_channels_in_tsc), Return(true))); - snmp_response_obj max_rings_in_tsc; + streets_snmp_cmd::snmp_response_obj max_rings_in_tsc; max_rings_in_tsc.val_int = 4; - max_rings_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_rings_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; EXPECT_CALL( *mock_client, process_snmp_request(ntcip_oids::MAX_RINGS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_rings_in_tsc), Return(true))); // Define Sequence Data - snmp_response_obj seq_data; - seq_data.val_string = {char(1),char(2)}; + streets_snmp_cmd::snmp_response_obj seq_data; + seq_data.val_string = {char(1),char(2),}; std::string seq_data_ring1_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(1); EXPECT_CALL(*mock_client, process_snmp_request(seq_data_ring1_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -68,7 +224,7 @@ namespace traffic_signal_controller_service for(int i = 1; i <= max_channels_in_tsc.val_int; ++i){ // Define Control Type - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_client, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -76,7 +232,7 @@ namespace traffic_signal_controller_service testing::Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = i; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_client, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -87,7 +243,7 @@ namespace traffic_signal_controller_service // Define get min green std::string min_green_oid = ntcip_oids::MINIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj min_green; + streets_snmp_cmd::snmp_response_obj min_green; min_green.val_int = 20; EXPECT_CALL(*mock_client, process_snmp_request(min_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(min_green), @@ -96,7 +252,7 @@ namespace traffic_signal_controller_service // Define get max green std::string max_green_oid = ntcip_oids::MAXIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj max_green; + streets_snmp_cmd::snmp_response_obj max_green; max_green.val_int = 30; EXPECT_CALL(*mock_client, process_snmp_request(max_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_green), @@ -105,7 +261,7 @@ namespace traffic_signal_controller_service // Define get yellow Duration std::string yellow_oid = ntcip_oids::YELLOW_CHANGE_PARAMETER + "." + std::to_string(i); - snmp_response_obj yellow_duration; + streets_snmp_cmd::snmp_response_obj yellow_duration; yellow_duration.val_int = 40; EXPECT_CALL(*mock_client, process_snmp_request(yellow_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(yellow_duration), @@ -115,7 +271,7 @@ namespace traffic_signal_controller_service // Define red clearance std::string red_clearance_oid = ntcip_oids::RED_CLEAR_PARAMETER + "." + std::to_string(i); - snmp_response_obj red_clearance_duration; + streets_snmp_cmd::snmp_response_obj red_clearance_duration; red_clearance_duration.val_int = 10; EXPECT_CALL(*mock_client, process_snmp_request(red_clearance_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(red_clearance_duration), @@ -123,7 +279,7 @@ namespace traffic_signal_controller_service //Define get concurrent phases std::string concurrent_phase_oid = ntcip_oids::PHASE_CONCURRENCY + "." + std::to_string(i); - snmp_response_obj concurrent_phase_resp; + streets_snmp_cmd::snmp_response_obj concurrent_phase_resp; if ( i == 1 || i == 2) { concurrent_phase_resp.val_string = {char(1), char(2)}; } @@ -154,6 +310,8 @@ namespace traffic_signal_controller_service EXPECT_TRUE(ped_phase_map.empty()); std::unordered_map vehicle_phase_map = worker.get_vehicle_phase_map(); EXPECT_FALSE(vehicle_phase_map.empty()); + std::unordered_map signal_group_map = worker.get_signal_group_map(); + EXPECT_FALSE(signal_group_map.empty()); //Test tsc_config_state std::shared_ptr tsc_config_state = worker.get_tsc_config_state(); @@ -276,24 +434,24 @@ namespace traffic_signal_controller_service // When phase to signal group map is empty, ASSERT_THROW(state.get_phase_number(1), monitor_states_exception); // Test get max channels - snmp_response_obj max_channels_in_tsc; + streets_snmp_cmd::snmp_response_obj max_channels_in_tsc; max_channels_in_tsc.val_int = 16; - max_channels_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_channels_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; EXPECT_CALL( *mock_snmp, process_snmp_request(ntcip_oids::MAX_CHANNELS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_channels_in_tsc), Return(true))); - snmp_response_obj max_rings_in_tsc; + streets_snmp_cmd::snmp_response_obj max_rings_in_tsc; max_rings_in_tsc.val_int = 4; - max_rings_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_rings_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; EXPECT_CALL( *mock_snmp, process_snmp_request(ntcip_oids::MAX_RINGS, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_rings_in_tsc), Return(true))); // Define Sequence Data - snmp_response_obj seq_data; + streets_snmp_cmd::snmp_response_obj seq_data; seq_data.val_string = {char(1),char(2), char(3), char(4)}; std::string seq_data_ring1_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(1); @@ -328,7 +486,7 @@ namespace traffic_signal_controller_service // switch(i) { case 3: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -336,7 +494,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 1; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -345,7 +503,7 @@ namespace traffic_signal_controller_service break; } case 7: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -353,7 +511,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 2; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -362,7 +520,7 @@ namespace traffic_signal_controller_service break; } case 9: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -370,7 +528,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 3; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -379,7 +537,7 @@ namespace traffic_signal_controller_service break; } case 14: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -387,7 +545,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 4; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -396,7 +554,7 @@ namespace traffic_signal_controller_service break; } case 11: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -404,7 +562,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 5; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -413,7 +571,7 @@ namespace traffic_signal_controller_service break; } case 6: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -421,7 +579,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 6; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -430,7 +588,7 @@ namespace traffic_signal_controller_service break; } case 15: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -438,7 +596,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 7; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -447,7 +605,7 @@ namespace traffic_signal_controller_service break; } case 1: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -455,7 +613,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 8; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -464,7 +622,7 @@ namespace traffic_signal_controller_service break; } default: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 0; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( @@ -474,7 +632,7 @@ namespace traffic_signal_controller_service // Define Control Source // Note this OID is not actually called for any non vehicle/pedestrian phases which is why the Times() assertion // is not included - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 0; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -489,7 +647,7 @@ namespace traffic_signal_controller_service // Define get min green std::string min_green_oid = ntcip_oids::MINIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj min_green; + streets_snmp_cmd::snmp_response_obj min_green; min_green.val_int = 20; EXPECT_CALL(*mock_snmp, process_snmp_request(min_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(min_green), @@ -498,7 +656,7 @@ namespace traffic_signal_controller_service // Define get max green std::string max_green_oid = ntcip_oids::MAXIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj max_green; + streets_snmp_cmd::snmp_response_obj max_green; max_green.val_int = 30; EXPECT_CALL(*mock_snmp, process_snmp_request(max_green_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_green), @@ -507,7 +665,7 @@ namespace traffic_signal_controller_service // Define get yellow Duration std::string yellow_oid = ntcip_oids::YELLOW_CHANGE_PARAMETER + "." + std::to_string(i); - snmp_response_obj yellow_duration; + streets_snmp_cmd::snmp_response_obj yellow_duration; yellow_duration.val_int = 40; EXPECT_CALL(*mock_snmp, process_snmp_request(yellow_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(yellow_duration), @@ -517,7 +675,7 @@ namespace traffic_signal_controller_service // Define red clearance std::string red_clearance_oid = ntcip_oids::RED_CLEAR_PARAMETER + "." + std::to_string(i); - snmp_response_obj red_clearance_duration; + streets_snmp_cmd::snmp_response_obj red_clearance_duration; red_clearance_duration.val_int = 10; EXPECT_CALL(*mock_snmp, process_snmp_request(red_clearance_oid , request_type , _) ).Times(1).WillRepeatedly(testing::DoAll( SetArgReferee<2>(red_clearance_duration), @@ -526,7 +684,7 @@ namespace traffic_signal_controller_service //Define get concurrent phases std::string concurrent_phase_oid = ntcip_oids::PHASE_CONCURRENCY + "." + std::to_string(i); - snmp_response_obj concurrent_phase_resp; + streets_snmp_cmd::snmp_response_obj concurrent_phase_resp; if ( i == 1 || i == 2) { concurrent_phase_resp.val_string = {char(5), char(6)}; } diff --git a/tsc_client_service/test/test_snmp_client.cpp b/tsc_client_service/test/test_snmp_client.cpp index 31d0d1967..847489f2f 100644 --- a/tsc_client_service/test/test_snmp_client.cpp +++ b/tsc_client_service/test/test_snmp_client.cpp @@ -13,9 +13,9 @@ TEST(test_snmp_client, test_process_snmp_request) snmp_client worker(dummy_ip, dummy_port); // Test GET - request_type request_type = request_type::GET; - snmp_response_obj int_response; - int_response.type = snmp_response_obj::response_type::INTEGER; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; + streets_snmp_cmd::snmp_response_obj int_response; + int_response.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; int_response.val_int = 0; // Expect get call to fail since we're communicating with invalid host EXPECT_FALSE(worker.process_snmp_request(test_oid, request_type, int_response)); @@ -26,7 +26,7 @@ TEST(test_snmp_client, test_process_snmp_request) EXPECT_FALSE(worker.process_snmp_request(test_oid, request_type, int_response)); // Test log_error - request_type = request_type::GET; + request_type = streets_snmp_cmd::REQUEST_TYPE::GET; snmp_pdu *response = nullptr; int status = STAT_TIMEOUT; worker.log_error(status, request_type, response); @@ -35,22 +35,22 @@ TEST(test_snmp_client, test_process_snmp_request) worker.log_error(status, request_type, response); // Test SET - request_type = request_type::SET; - snmp_response_obj set_value; - int_response.type = snmp_response_obj::response_type::INTEGER; + request_type = streets_snmp_cmd::REQUEST_TYPE::SET; + streets_snmp_cmd::snmp_response_obj set_value; + int_response.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; int_response.val_int = 10; // Expect set call to fail since we're communicating with invalid host EXPECT_FALSE(worker.process_snmp_request(test_oid, request_type, set_value)); // Test log_error status = STAT_TIMEOUT; - request_type = request_type::SET; + request_type = streets_snmp_cmd::REQUEST_TYPE::SET; status = -7; //Random error value worker.log_error(status, request_type,response); // Invalid Request type - request_type = request_type::OTHER; + request_type = streets_snmp_cmd::REQUEST_TYPE::OTHER; EXPECT_FALSE(worker.process_snmp_request(test_oid, request_type, set_value)); } diff --git a/tsc_client_service/test/test_spat_projection_mode.cpp b/tsc_client_service/test/test_spat_projection_mode.cpp new file mode 100644 index 000000000..967fd6708 --- /dev/null +++ b/tsc_client_service/test/test_spat_projection_mode.cpp @@ -0,0 +1,12 @@ +#include +#include "spat_projection_mode.h" + +using namespace traffic_signal_controller_service; + +TEST(test_spat_projection_mode, test_from_int ) { + ASSERT_EQ( SPAT_PROJECTION_MODE::NO_PROJECTION, spat_projection_mode_from_int(0)); + ASSERT_EQ( SPAT_PROJECTION_MODE::DPP_PROJECTION, spat_projection_mode_from_int(1)); + ASSERT_EQ( SPAT_PROJECTION_MODE::FIXED_TIMING_PROJECTION, spat_projection_mode_from_int(2)); + ASSERT_EQ( SPAT_PROJECTION_MODE::NO_PROJECTION, spat_projection_mode_from_int(-1)); + ASSERT_EQ( SPAT_PROJECTION_MODE::NO_PROJECTION, spat_projection_mode_from_int(5)); +} diff --git a/tsc_client_service/test/test_tsc_service.cpp b/tsc_client_service/test/test_tsc_service.cpp index 9247042cd..eaa6041e2 100644 --- a/tsc_client_service/test/test_tsc_service.cpp +++ b/tsc_client_service/test/test_tsc_service.cpp @@ -31,33 +31,37 @@ namespace traffic_signal_controller_service service.spat_producer = spat_producer; service.tsc_config_producer = tsc_config_producer; service.desired_phase_plan_consumer = dpp_consumer; + service.phase_control_schedule_consumer = std::make_shared(); service.snmp_client_ptr = mock_snmp; - setenv("SIMULATION_MODE", "FALSE", 1); - setenv("CONFIG_FILE_PATH", "../manifest.json", 1); + service.tsc_state_ptr = std::make_shared(mock_snmp); + setenv(streets_service::SIMULATION_MODE_ENV.c_str(), "TRUE", 1); + setenv(streets_service::TIME_SYNC_TOPIC_ENV.c_str(), "time_sync", 1); + setenv(streets_service::CONFIG_FILE_PATH_ENV.c_str(), "../test/test_files/manifest.json", 1); + setenv(streets_service::LOGS_DIRECTORY_ENV.c_str(), "../logs/", 1); } void mock_tsc_ntcip() { // gmock SNMP response --------------------------------------------------------------------------------------------------------------------- // Test get max channels - snmp_response_obj max_channels_in_tsc; + streets_snmp_cmd::snmp_response_obj max_channels_in_tsc; max_channels_in_tsc.val_int = 16; - max_channels_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_channels_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; - request_type request_type = request_type::GET; + auto request_type= streets_snmp_cmd::REQUEST_TYPE::GET; EXPECT_CALL( *mock_snmp, process_snmp_request(ntcip_oids::MAX_CHANNELS, request_type , _) ).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_channels_in_tsc), Return(true))); - snmp_response_obj max_rings_in_tsc; + streets_snmp_cmd::snmp_response_obj max_rings_in_tsc; max_rings_in_tsc.val_int = 4; - max_rings_in_tsc.type = snmp_response_obj::response_type::INTEGER; + max_rings_in_tsc.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; EXPECT_CALL( *mock_snmp, process_snmp_request(ntcip_oids::MAX_RINGS, request_type , _) ).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_rings_in_tsc), Return(true))); // Define Sequence Data - snmp_response_obj seq_data; + streets_snmp_cmd::snmp_response_obj seq_data; seq_data.val_string = {char(1),char(2), char(3), char(4)}; std::string seq_data_ring1_oid = ntcip_oids::SEQUENCE_DATA + "." + "1" + "." + std::to_string(1); @@ -92,7 +96,7 @@ namespace traffic_signal_controller_service // switch(i) { case 1: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -100,7 +104,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 1; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -109,7 +113,7 @@ namespace traffic_signal_controller_service break; } case 2: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -117,7 +121,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 2; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -126,7 +130,7 @@ namespace traffic_signal_controller_service break; } case 3: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -134,7 +138,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 3; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -143,7 +147,7 @@ namespace traffic_signal_controller_service break; } case 4: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -151,7 +155,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 4; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -160,7 +164,7 @@ namespace traffic_signal_controller_service break; } case 5: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -168,7 +172,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 5; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -177,7 +181,7 @@ namespace traffic_signal_controller_service break; } case 6: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -185,7 +189,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 6; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -194,7 +198,7 @@ namespace traffic_signal_controller_service break; } case 7: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -202,7 +206,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 7; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -211,7 +215,7 @@ namespace traffic_signal_controller_service break; } case 8: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 2; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -219,7 +223,7 @@ namespace traffic_signal_controller_service Return(true))); // Define Control Source - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 8; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -228,7 +232,7 @@ namespace traffic_signal_controller_service break; } default: { - snmp_response_obj channel_control_resp; + streets_snmp_cmd::snmp_response_obj channel_control_resp; channel_control_resp.val_int = 0; std::string channel_control_oid = ntcip_oids::CHANNEL_CONTROL_TYPE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(channel_control_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -238,7 +242,7 @@ namespace traffic_signal_controller_service // Define Control Source // Note this OID is not actually called for any non vehicle/pedestrian phases which is why the Times() assertion // is not included - snmp_response_obj control_source_resp; + streets_snmp_cmd::snmp_response_obj control_source_resp; control_source_resp.val_int = 0; std::string control_source_oid = ntcip_oids::CHANNEL_CONTROL_SOURCE_PARAMETER + "." + std::to_string(i); EXPECT_CALL(*mock_snmp, process_snmp_request(control_source_oid, request_type , _) ).WillRepeatedly(testing::DoAll( @@ -253,7 +257,7 @@ namespace traffic_signal_controller_service // Define get min green std::string min_green_oid = ntcip_oids::MINIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj min_green; + streets_snmp_cmd::snmp_response_obj min_green; min_green.val_int = 5; EXPECT_CALL(*mock_snmp, process_snmp_request(min_green_oid , request_type , _) ).WillRepeatedly(testing::DoAll( SetArgReferee<2>(min_green), @@ -262,7 +266,7 @@ namespace traffic_signal_controller_service // Define get max green std::string max_green_oid = ntcip_oids::MAXIMUM_GREEN + "." + std::to_string(i); - snmp_response_obj max_green; + streets_snmp_cmd::snmp_response_obj max_green; max_green.val_int = 300; EXPECT_CALL(*mock_snmp, process_snmp_request(max_green_oid , request_type , _) ).WillRepeatedly(testing::DoAll( SetArgReferee<2>(max_green), @@ -271,7 +275,7 @@ namespace traffic_signal_controller_service // Define get yellow Duration std::string yellow_oid = ntcip_oids::YELLOW_CHANGE_PARAMETER + "." + std::to_string(i); - snmp_response_obj yellow_duration; + streets_snmp_cmd::snmp_response_obj yellow_duration; yellow_duration.val_int = 10; EXPECT_CALL(*mock_snmp, process_snmp_request(yellow_oid , request_type , _) ).WillRepeatedly(testing::DoAll( SetArgReferee<2>(yellow_duration), @@ -281,7 +285,7 @@ namespace traffic_signal_controller_service // Define red clearance std::string red_clearance_oid = ntcip_oids::RED_CLEAR_PARAMETER + "." + std::to_string(i); - snmp_response_obj red_clearance_duration; + streets_snmp_cmd::snmp_response_obj red_clearance_duration; red_clearance_duration.val_int = 15; EXPECT_CALL(*mock_snmp, process_snmp_request(red_clearance_oid , request_type , _) ).WillRepeatedly(testing::DoAll( SetArgReferee<2>(red_clearance_duration), @@ -290,7 +294,7 @@ namespace traffic_signal_controller_service //Define get concurrent phases std::string concurrent_phase_oid = ntcip_oids::PHASE_CONCURRENCY + "." + std::to_string(i); - snmp_response_obj concurrent_phase_resp; + streets_snmp_cmd::snmp_response_obj concurrent_phase_resp; if ( i == 1 || i == 2) { concurrent_phase_resp.val_string = {char(5), char(6)}; } @@ -382,9 +386,11 @@ namespace traffic_signal_controller_service service.initialize_spat("test_intersection",1234,std::unordered_map{ {1,8},{2,7},{3,6},{4,5},{5,4},{6,3},{7,2},{8,1}} ); ASSERT_TRUE(service.initialize_spat_worker("127.0.0.1",3456,2,false)); + service.produce_spat_json(); } + TEST_F(tsc_service_test, test_tsc_control){ // Initialize clock singleton in realtime mode @@ -397,29 +403,31 @@ namespace traffic_signal_controller_service .WillRepeatedly(testing::DoAll(testing::Return(true))); // Define command - snmp_response_obj set_val; - set_val.type = snmp_response_obj::response_type::INTEGER; + streets_snmp_cmd::snmp_response_obj set_val; + set_val.type = streets_snmp_cmd::RESPONSE_TYPE::INTEGER; uint64_t start_time = 0; - snmp_cmd_struct::control_type control_type = snmp_cmd_struct::control_type::Hold; - snmp_cmd_struct hold_command(mock_snmp, start_time, control_type, 0); - std::queue tsc_set_command_queue; + streets_snmp_cmd::PHASE_CONTROL_TYPE control_type = streets_snmp_cmd::PHASE_CONTROL_TYPE::HOLD_VEH_PHASES; + streets_snmp_cmd::snmp_cmd_struct hold_command(start_time, control_type, 0); + std::queue tsc_set_command_queue; tsc_set_command_queue.push(hold_command); service.tsc_set_command_queue_ = tsc_set_command_queue; - EXPECT_THROW(service.set_tsc_hold_and_omit(), control_tsc_state_exception); + auto tsc_state_ptr = std::make_shared(service.snmp_client_ptr ); + service.control_tsc_state_ptr_ = std::make_shared(service.snmp_client_ptr, tsc_state_ptr); + EXPECT_THROW(service.set_tsc_hold_and_omit_forceoff_call(), control_tsc_state_exception); ASSERT_EQ(1, service.tsc_set_command_queue_.size()); // Test control_tsc_phases service.control_tsc_phases(); ASSERT_EQ(0, service.tsc_set_command_queue_.size()); start_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count() + 100; - snmp_cmd_struct hold_command_2(mock_snmp, start_time, control_type, 0); - std::queue tsc_set_command_queue_2; + streets_snmp_cmd::snmp_cmd_struct hold_command_2(start_time, control_type, 0); + std::queue tsc_set_command_queue_2; tsc_set_command_queue_2.push(hold_command_2); service.tsc_set_command_queue_ = tsc_set_command_queue_2; ASSERT_EQ(1, service.tsc_set_command_queue_.size()); - EXPECT_NO_THROW(service.set_tsc_hold_and_omit()); + EXPECT_NO_THROW(service.set_tsc_hold_and_omit_forceoff_call()); ASSERT_EQ(0, service.tsc_set_command_queue_.size()); } @@ -473,12 +481,19 @@ namespace traffic_signal_controller_service TEST_F(tsc_service_test, test_configure_snmp_cmd_logger) { - // Initialize clock singleton in realtime mode streets_service::streets_clock_singleton::create(false); service.configure_snmp_cmd_logger(); - auto logger = spdlog::get("snmp_cmd_logger"); + auto logger = spdlog::get( service.SNMP_COMMAND_LOGGER_NAME); + ASSERT_EQ(logger->level(), spdlog::level::info); + } + + + TEST_F(tsc_service_test, test_configure_veh_ped_call_logger) { + service.configure_veh_ped_call_logger(); + auto logger = spdlog::get( service.VEH_PED_CALL_LOGGER_NAME); EXPECT_TRUE(logger != nullptr); + ASSERT_EQ(logger->level(), spdlog::level::info); } // Test tsc_service initialize without mocking snmp responses TEST_F(tsc_service_test, test_initialization_no_mock_response_from_snmp_client) {