├── .clang-format ├── .dockerignore ├── .gitattributes ├── .github ├── ISSUE_TEMPLATE │ └── bug_report.md └── workflows │ ├── build.yaml │ ├── build_and_test.yaml │ ├── clang-tidy.yaml │ ├── code-checks.yaml │ ├── cppcheck.yaml │ ├── format.yml │ └── push_and_version_tagg.yml ├── .gitignore ├── .gitmodules ├── .pre-commit-config.yaml ├── .readthedocs.yaml ├── Dockerfile ├── Doxyfile ├── LICENSE.md ├── README.md ├── SECURITY.md ├── atos ├── CMakeLists.txt ├── cmake │ ├── cpack │ │ ├── CMakeLists.txt │ │ ├── CPack.MaestroDescription.txt │ │ ├── postinst │ │ ├── postrm │ │ └── prerm │ └── modules │ │ ├── FindesminiLib.cmake │ │ └── FindesminiRMLib.cmake ├── common │ ├── CMakeLists.txt │ ├── CRSTransformation.cpp │ ├── CRSTransformation.hpp │ ├── journal.cpp │ ├── journal.hpp │ ├── loggable.hpp │ ├── module.cpp │ ├── module.hpp │ ├── objectconfig.cpp │ ├── objectconfig.hpp │ ├── osihandler │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── osi_handler.cpp │ │ └── osi_handler.hpp │ ├── regexpatterns.hpp │ ├── roschannels │ │ ├── cartesiantrajectorychannel.hpp │ │ ├── commandchannels.hpp │ │ ├── controlsignalchannel.hpp │ │ ├── customcommandaction.hpp │ │ ├── gnsspathchannel.hpp │ │ ├── monitorchannel.hpp │ │ ├── navsatfixchannel.hpp │ │ ├── objstatechangechannel.hpp │ │ ├── pathchannel.hpp │ │ ├── pointcloudchannel.hpp │ │ ├── remotecontrolchannels.hpp │ │ ├── roschannel.hpp │ │ ├── scenariochannel.hpp │ │ ├── statechange.hpp │ │ └── test_channels.hpp │ ├── shmem │ │ ├── CMakeLists.txt │ │ ├── shmem.cpp │ │ └── shmem.h │ ├── sockets │ │ ├── CMakeLists.txt │ │ ├── canhandler.cpp │ │ ├── canhandler.hpp │ │ ├── client.cpp │ │ ├── client.hpp │ │ ├── server.cpp │ │ ├── server.hpp │ │ ├── socket.cpp │ │ ├── socket.hpp │ │ ├── socketexceptions.hpp │ │ ├── tcphandler.cpp │ │ ├── tcphandler.hpp │ │ ├── test_can.cpp │ │ ├── test_socket.cpp │ │ ├── udphandler.cpp │ │ └── udphandler.hpp │ ├── testUtils │ │ └── testUtils.hpp │ ├── tests │ │ └── test_relativetrajectory.cpp │ ├── time │ │ ├── CMakeLists.txt │ │ ├── atosTime.c │ │ └── atosTime.h │ ├── trajectory.cpp │ ├── trajectory.hpp │ ├── type.cpp │ ├── type.h │ ├── util.c │ └── util.h ├── launch │ ├── launch_basic.py │ ├── launch_experimental.py │ ├── launch_full.py │ ├── launch_graphical.py │ ├── launch_integration_testing.py │ └── launch_utils │ │ ├── generate_cert.py │ │ ├── launch_base.py │ │ └── validate_files.py ├── modules │ ├── BackToStart │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ └── backtostart.hpp │ │ ├── src │ │ │ ├── backtostart.cpp │ │ │ └── main.cpp │ │ └── tests │ │ │ ├── main.cpp │ │ │ └── test_backtostart.cpp │ ├── DirectControl │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ └── directcontrol.hpp │ │ └── src │ │ │ ├── directcontrol.cpp │ │ │ └── main.cpp │ ├── EsminiAdapter │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ ├── esminiadapter.hpp │ │ │ └── string_utility.hpp │ │ └── src │ │ │ ├── esminiadapter.cpp │ │ │ └── main.cpp │ ├── IntegrationTesting │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ ├── integrationtesting.hpp │ │ │ ├── integrationtestingfactory.hpp │ │ │ ├── integrationtestinghandler.hpp │ │ │ └── scenarioexecution.hpp │ │ └── src │ │ │ ├── integrationtesting.cpp │ │ │ ├── integrationtestingfactory.cpp │ │ │ ├── integrationtestinghandler.cpp │ │ │ ├── main.cpp │ │ │ └── scenarioexecution.cpp │ ├── JournalControl │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ ├── journalcontrol.hpp │ │ │ ├── journalmodel.hpp │ │ │ └── journalmodelcollection.hpp │ │ └── src │ │ │ ├── journalcontrol.cpp │ │ │ ├── journalmodel.cpp │ │ │ ├── journalmodelcollection.cpp │ │ │ └── main.cpp │ ├── MQTTBridge │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ ├── Imqtt2ros.hpp │ │ │ ├── Iros2mqtt.hpp │ │ │ └── mqttbridge.hpp │ │ └── src │ │ │ ├── main.cpp │ │ │ └── mqttbridge.cpp │ ├── MonrRelay │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ └── monr_relay.hpp │ │ └── src │ │ │ ├── main.cpp │ │ │ └── monr_relay.cpp │ ├── OSIAdapter │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ ├── osiadapter.hpp │ │ │ └── serverfactory.hpp │ │ └── src │ │ │ ├── main.cpp │ │ │ ├── osiadapter.cpp │ │ │ └── serverfactory.cpp │ ├── ObjectControl │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ ├── channel.hpp │ │ │ ├── objectconnection.hpp │ │ │ ├── objectcontrol.hpp │ │ │ ├── objectlistener.hpp │ │ │ ├── relativeanchor.hpp │ │ │ ├── relativetestobject.hpp │ │ │ ├── sml.hpp │ │ │ ├── statemachine.hpp │ │ │ └── testobject.hpp │ │ └── src │ │ │ ├── channel.cpp │ │ │ ├── dump_state_machine.cpp │ │ │ ├── main.cpp │ │ │ ├── objectconnection.cpp │ │ │ ├── objectcontrol.cpp │ │ │ ├── objectlistener.cpp │ │ │ ├── relativeanchor.cpp │ │ │ ├── relativetestobject.cpp │ │ │ └── testobject.cpp │ ├── OpenScenarioGateway │ │ ├── custom_command_action.py │ │ ├── openscenariogateway.py │ │ ├── storyboard_handler.py │ │ └── tests │ │ │ ├── resources │ │ │ ├── Catalogs │ │ │ │ └── Vehicles │ │ │ │ │ ├── Controllers │ │ │ │ │ └── ControllerCatalog.xosc │ │ │ │ │ └── VehicleCatalog.xosc │ │ │ ├── odr │ │ │ │ └── GaragePlanHighRes.xodr │ │ │ └── osc │ │ │ │ └── GaragePlanScenario.xosc │ │ │ ├── test_openscenariogateway.py │ │ │ └── test_storyboard_handler.py │ ├── PointcloudPublisher │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ └── pointcloudpublisher.hpp │ │ └── src │ │ │ ├── main.cpp │ │ │ └── pointcloudpublisher.cpp │ ├── RESTBridge │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ └── restbridge.hpp │ │ └── src │ │ │ ├── main.cpp │ │ │ └── restbridge.cpp │ ├── SampleModule │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ └── samplemodule.hpp │ │ ├── src │ │ │ ├── main.cpp │ │ │ └── samplemodule.cpp │ │ └── test │ │ │ ├── main.cpp │ │ │ └── test_samplemodule.cpp │ ├── SqlBridge │ │ └── sqlbridge.py │ ├── SystemControl │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ └── systemcontrol.hpp │ │ └── src │ │ │ ├── main.cpp │ │ │ └── systemcontrol.cpp │ ├── TrajectoryletStreamer │ │ ├── CMakeLists.txt │ │ ├── inc │ │ │ ├── trajectoryletstreamer.hpp │ │ │ └── trajectorypublisher.hpp │ │ └── src │ │ │ ├── main.cpp │ │ │ ├── trajectoryletstreamer.cpp │ │ │ └── trajectorypublisher.cpp │ ├── __init__.py │ └── conftest.py └── package.xml ├── atos_gui ├── atos_gui │ ├── __init__.py │ ├── configpanel │ │ ├── __init__.py │ │ ├── configpanel.py │ │ └── local_file_picker.py │ ├── controlpanel │ │ ├── __init__.py │ │ └── controlpanel.py │ ├── images │ │ └── favicon.ico │ ├── main.py │ └── objectpanel │ │ ├── __init__.py │ │ └── objectpanel.py ├── launch │ └── gui.py ├── package.xml ├── requirements.txt ├── setup.cfg └── setup.py ├── conf ├── Catalogs │ ├── Controllers │ │ └── ControllerCatalog.xosc │ ├── Maneuvers │ │ └── HWManeuvers.xosc │ ├── Pedestrians │ │ └── PedestrianCatalog.xosc │ ├── Routes │ │ └── RoutesAtMultiIntersections.xosc │ └── Vehicles │ │ └── VehicleCatalog.xosc ├── README.md ├── conf │ ├── atos-param-schema.json │ └── params.yaml ├── odr │ ├── GaragePlanHighRes.xodr │ └── README.md ├── osc │ └── GaragePlanScenario.xosc └── rviz │ └── objectvisualization.rviz ├── docker-compose-bridge.yml ├── docker-compose-test.yml ├── docker-compose.yml ├── docs ├── About │ └── aboutATOS.md ├── Contributing │ ├── Contributing.md │ ├── Feature Requests │ │ └── Feature_requests.md │ ├── Issues │ │ └── issues.md │ ├── Pull Requests │ │ └── pull_requests.md │ └── Testing │ │ └── testing.md ├── Images │ ├── BackToStart_after.png │ └── BackToStart_before.png ├── Installation │ ├── connected.gif │ ├── controlpanel.png │ ├── installation.md │ ├── isoobject_preconnect.png │ └── quickstart.md ├── README.md ├── Usage │ ├── GUI │ │ ├── controlpanel.md │ │ └── foxglove.md │ ├── How-to │ │ ├── configuration.md │ │ └── docker_networking.md │ ├── Modules │ │ ├── BackToStart.md │ │ ├── DirectControl.md │ │ ├── EsminiAdapter.md │ │ ├── IntegrationTesting.md │ │ ├── JournalControl.md │ │ ├── MQTTBridge.md │ │ ├── OSIAdapter.md │ │ ├── ObjectControl.md │ │ ├── OpenScenarioGateway.md │ │ ├── PointcloudPublisher.md │ │ ├── SampleModule.md │ │ └── TrajectoryletStreamer.md │ └── Summary │ │ └── summary_of_states.md ├── index.md ├── requirements.in ├── requirements.txt └── res │ ├── ATOS-UML.png │ ├── ATOS_architecture.drawio.png │ ├── ATOS_architecture.xml │ ├── ATOS_icon.png │ └── ATOS_icon.svg ├── format.py ├── mkdocs.yml ├── plugins ├── foxglove │ ├── Map and 3D layout.json │ ├── Map, 3D and control layout.json │ ├── astazero.atos_control_panel-0.0.1.foxe │ └── astazero.monr_to_pose-0.0.1.foxe └── rviz2 │ ├── CMakeLists.txt │ ├── include │ └── object_monitor_display.hpp │ ├── package.xml │ ├── plugin_description.xml │ └── src │ └── object_monitor_display.cpp ├── scripts ├── installation │ ├── dependencies.txt │ ├── install_atos.sh │ ├── install_deps.sh │ ├── install_functions.sh │ └── requirements.txt ├── integration_testing │ └── run_scenario_test.py └── openscenario │ ├── README.md │ ├── garage_plan_test_scenario.py │ └── requirements.txt └── setup_atos.sh /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Mozilla 2 | InsertNewlineAtEOF: true 3 | UseTab: Always 4 | IndentWidth: 4 5 | TabWidth: 4 6 | ColumnLimit: 120 7 | AccessModifierOffset: -4 8 | AlignConsecutiveAssignments: Consecutive 9 | AllowShortBlocksOnASingleLine: Empty 10 | AllowShortFunctionsOnASingleLine: Empty 11 | AlwaysBreakAfterDefinitionReturnType: None 12 | AlwaysBreakAfterReturnType: None 13 | BraceWrapping: 14 | SplitEmptyFunction: false 15 | BreakBeforeBraces: Custom 16 | BreakConstructorInitializers: AfterColon 17 | Cpp11BracedListStyle: true 18 | FixNamespaceComments: true 19 | PackConstructorInitializers: Never 20 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | .github 2 | .vscode 3 | build 4 | docs 5 | install 6 | plugins 7 | Dockerfile -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | conf/traj/** linguist-detectable=false 2 | conf/geofence/** linguist-detectable=false 3 | conf/** linguist-detectable=false 4 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: File a bug report 4 | title: '' 5 | labels: bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Branch or release information (please complete the following information):** 27 | - Branch/release name: [eg. main, dev or version 1.0.0] 28 | 29 | **Desktop (please complete the following information):** 30 | - OS: [e.g. ubuntu, arch] 31 | - Browser [e.g. chrome, safari] 32 | - Version [e.g. 22] 33 | 34 | **Additional context** 35 | Add any other context about the problem here. 36 | -------------------------------------------------------------------------------- /.github/workflows/build.yaml: -------------------------------------------------------------------------------- 1 | name: Docker Image CI 2 | 3 | on: workflow_call 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-latest 8 | container: ros:humble 9 | steps: 10 | - name: Check out repository 11 | uses: actions/checkout@v4 12 | with: 13 | submodules: recursive 14 | - run: git config --system --add safe.directory "$PWD" 15 | 16 | - name: Restore build cache 17 | id: cache-build 18 | uses: actions/cache@v4 19 | with: 20 | path: | 21 | /github/home/atos_ws/build 22 | /github/home/atos_ws/install 23 | key: ${{ runner.os }}-build 24 | 25 | - name: Install ATOS 26 | run: | 27 | ./setup_atos.sh 28 | shell: bash 29 | 30 | - name: Print build folder 31 | run: | 32 | ls -R /github/home/atos_ws/build 33 | shell: bash 34 | 35 | - name: Upload build folder 36 | uses: actions/upload-artifact@v4 37 | with: 38 | name: build-folder 39 | path: /github/home/atos_ws/build -------------------------------------------------------------------------------- /.github/workflows/build_and_test.yaml: -------------------------------------------------------------------------------- 1 | name: Docker Image CI 2 | 3 | on: 4 | push: 5 | branches: [ "dev*", "master" ] 6 | pull_request: 7 | branches: [ "dev" ] 8 | 9 | concurrency: 10 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} 11 | cancel-in-progress: true 12 | 13 | env: 14 | TEST_TAG: astazero/atos_docker_env:test 15 | 16 | jobs: 17 | build_and_test_docker_image: 18 | runs-on: ubuntu-latest 19 | steps: 20 | - name: Set up Docker Buildx 21 | uses: docker/setup-buildx-action@v3 22 | 23 | - name: Checkout 24 | uses: actions/checkout@v4 25 | with: 26 | submodules: recursive 27 | 28 | - name: Build Docker image 29 | uses: docker/build-push-action@v5 30 | with: 31 | context: . 32 | file: Dockerfile 33 | load: true 34 | tags: ${{ env.TEST_TAG }} 35 | cache-from: type=gha 36 | cache-to: type=gha,mode=max 37 | 38 | - name: Run colcon test on docker image 39 | run: | 40 | docker run --rm ${{ env.TEST_TAG }} bash -c "colcon test --return-code-on-test-failure --event-handlers console_cohesion+" 41 | 42 | - name: Run scenario integration test 43 | run: | 44 | docker compose -f docker-compose-test.yml up --abort-on-container-exit -------------------------------------------------------------------------------- /.github/workflows/clang-tidy.yaml: -------------------------------------------------------------------------------- 1 | name: clang-tidy-review 2 | 3 | # You can be more specific, but it currently only works on pull requests 4 | on: 5 | workflow_call: 6 | inputs: 7 | target_files: 8 | description: 'The list of files to be checked by clang-tidy' 9 | required: true 10 | default: '' 11 | type: string 12 | 13 | jobs: 14 | clang-tidy: 15 | runs-on: ubuntu-latest 16 | container: ros:humble 17 | steps: 18 | - name: Check out repository 19 | uses: actions/checkout@v4 20 | with: 21 | submodules: recursive 22 | 23 | - name: Prepare atos workspace 24 | run: | 25 | mkdir -p $HOME/atos_ws/src 26 | ln -s $GITHUB_WORKSPACE $HOME/atos_ws/src/atos 27 | 28 | - name: Download build folder 29 | uses: actions/download-artifact@v4 30 | with: 31 | name: build-folder 32 | path: /github/home/atos_ws/build 33 | 34 | - name: Install Clang-Tidy 35 | run: | 36 | sudo apt-get -yqq update 37 | sudo apt-get -yqq install clang-tidy libomp-dev 38 | shell: bash 39 | 40 | - name: Run clang-tidy linter 41 | if: ${{ inputs.target_files != '' }} 42 | env: 43 | ALL_CHANGED_FILES: ${{ inputs.target_files }} 44 | run: | 45 | echo "running clang-tidy on $ALL_CHANGED_FILES" 46 | ls $HOME/atos_ws/build/ 47 | mkdir /tmp/clang-tidy-result 48 | clang-tidy -p $HOME/atos_ws/build/ -export-fixes /tmp/clang-tidy-result/fixes.yaml --checks=cppcoreguidelines-*,clang-analyzer-* ${ALL_CHANGED_FILES} || true 49 | echo "${{ github.event.number }}" > /tmp/clang-tidy-result/pr-id.txt 50 | echo "${{ github.event.pull_request.head.repo.full_name }}" > /tmp/clang-tidy-result/pr-head-repo.txt 51 | echo "${{ github.event.pull_request.head.ref }}" > /tmp/clang-tidy-result/pr-head-ref.txt 52 | shell: bash 53 | 54 | - name: Check if the fixes.yaml file exists 55 | id: check-fixes-yaml-existence 56 | uses: autowarefoundation/autoware-github-actions/check-file-existence@v1 57 | with: 58 | files: /tmp/clang-tidy-result/fixes.yaml 59 | 60 | - name: Upload clang-tidy result 61 | if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} 62 | uses: actions/upload-artifact@v4 63 | with: 64 | name: clang-tidy-result 65 | path: /tmp/clang-tidy-result/ 66 | -------------------------------------------------------------------------------- /.github/workflows/code-checks.yaml: -------------------------------------------------------------------------------- 1 | name: Clang 2 | 3 | on: [pull_request] 4 | 5 | jobs: 6 | build: 7 | uses: ./.github/workflows/build.yaml 8 | 9 | file-changes: 10 | runs-on: ubuntu-latest 11 | outputs: 12 | all: ${{ steps.changed-files.outputs.all }} 13 | cpp: ${{ steps.cpp-files.outputs.CPP }} 14 | steps: 15 | - name: Check out repository 16 | uses: actions/checkout@v4 17 | with: 18 | submodules: recursive 19 | 20 | - name: Get changed files 21 | id: changed-files 22 | uses: jitterbit/get-changed-files@v1 23 | 24 | - name: Filter on only cpp, hpp, c, and h files 25 | id: cpp-files 26 | run: | 27 | input_files=(${{ steps.changed-files.outputs.all }}) 28 | echo "Input files: $input_files" 29 | cpp_files="" 30 | for f in "${input_files[@]}"; do 31 | if [[ $f == *.cpp || $f == *.hpp || $f == *.c || $f == *.h ]]; then 32 | echo "File ending correct: $f" 33 | cpp_files+="$f " 34 | else 35 | echo "File ending incorrect: $f" 36 | fi 37 | done 38 | echo "Target files: $cpp_files" 39 | 40 | { 41 | echo 'CPP<> $GITHUB_OUTPUT 45 | 46 | - name: Print all files 47 | run: | 48 | echo ${{ steps.cpp-files.outputs.CPP }} 49 | 50 | clang-tidy: 51 | uses: ./.github/workflows/clang-tidy.yaml 52 | needs: [build, file-changes] 53 | with: 54 | target_files: ${{ needs.file-changes.outputs.cpp }} 55 | 56 | cppcheck: 57 | uses: ./.github/workflows/cppcheck.yaml 58 | -------------------------------------------------------------------------------- /.github/workflows/cppcheck.yaml: -------------------------------------------------------------------------------- 1 | name: cppcheck 2 | 3 | on: workflow_call 4 | 5 | jobs: 6 | cppcheck: 7 | runs-on: ubuntu-latest 8 | container: ros:humble 9 | steps: 10 | - name: Check out repository 11 | uses: actions/checkout@v4 12 | with: 13 | submodules: recursive 14 | 15 | - name: Install cppcheck 16 | run: | 17 | sudo apt-get -yqq update 18 | sudo apt-get -yqq install cppcheck 19 | shell: bash 20 | 21 | - name: Run cppcheck. 22 | id: cppcheck 23 | run: | 24 | mkdir /tmp/cppcheck-result 25 | cppcheck --output-file=/tmp/cppcheck-result/cppcheck-result.txt . 26 | 27 | - name: Check if the cppcheck-result.txt file is empty 28 | id: check-cppcheck-result-empty 29 | run: | 30 | if [ -s /tmp/cppcheck-result/cppcheck-result.txt ]; then 31 | echo "empty=false" >> "$GITHUB_OUTPUT" 32 | else 33 | echo "empty=true" >> "$GITHUB_OUTPUT" 34 | fi 35 | shell: bash 36 | 37 | - name: Upload cppcheck result 38 | if: ${{ steps.check-cppcheck-result-empty.outputs.empty == 'false' }} 39 | uses: actions/upload-artifact@v4 40 | with: 41 | name: cppcheck-result 42 | path: /tmp/cppcheck-result/ 43 | 44 | - name: Mark the workflow as failed if the cppcheck-results.txt file contains errors 45 | if: ${{ steps.check-cppcheck-result-empty.outputs.empty == 'false' }} 46 | run: | 47 | exit 1 48 | shell: bash 49 | -------------------------------------------------------------------------------- /.github/workflows/format.yml: -------------------------------------------------------------------------------- 1 | name: format 2 | 3 | on: 4 | pull_request: 5 | branches: [ "dev" ] 6 | 7 | jobs: 8 | build: 9 | runs-on: ubuntu-24.04 10 | steps: 11 | - uses: actions/checkout@v4 12 | with: 13 | fetch-depth: 0 14 | 15 | - name: Fetch 16 | run: git fetch origin dev 17 | 18 | - name: CheckFormat 19 | run: ./format.py --check 20 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Object files 2 | *.o 3 | *.ko 4 | *.obj 5 | *.elf 6 | 7 | # Compiled python code 8 | *.pyc 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Libraries 15 | *.lib 16 | *.a 17 | *.la 18 | *.lo 19 | 20 | # Shared objects (inc. Windows DLLs) 21 | *.dll 22 | *.so 23 | *.so.* 24 | *.dylib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | *.i*86 31 | *.x86_64 32 | *.hex 33 | 34 | # Debug files 35 | *.dSYM/ 36 | *.su 37 | 38 | # User files 39 | *.user 40 | *.orig 41 | *kdev4 42 | # Catalogs 43 | build*/ 44 | .vscode*/ 45 | PlaygroundPlatform*/ 46 | .cache*/ 47 | CMakeFiles*/ 48 | 49 | # Autosave files 50 | *.save 51 | *.autosave 52 | *log.txt 53 | 54 | # CMake 55 | CMakeCache.txt 56 | 57 | # Generated files 58 | scripts/*.xosc 59 | scripts/openscenario/*.xosc 60 | docs/html/* 61 | log/ 62 | 63 | # 64 | *.env 65 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "atos/iso22133"] 2 | path = atos/iso22133 3 | url = git@github.com:RI-SE/iso22133.git 4 | [submodule "atos_interfaces"] 5 | path = atos_interfaces 6 | url = git@github.com:RI-SE/atos_interfaces.git 7 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | fail_fast: false 2 | repos: 3 | - repo: https://github.com/pocc/pre-commit-hooks 4 | rev: v1.3.5 5 | hooks: 6 | - id: clang-format 7 | args: [--style=LLVM, -i] 8 | - id: cppcheck 9 | - repo: https://github.com/ambv/black 10 | rev: 24.4.2 11 | hooks: 12 | - id: black -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | # .readthedocs.yaml 2 | # Read the Docs configuration file 3 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 4 | 5 | # Required 6 | version: 2 7 | 8 | # Set the version of Python and other tools you might need 9 | build: 10 | os: ubuntu-22.04 11 | tools: 12 | python: "3.8" 13 | apt_packages: 14 | - graphviz 15 | jobs: 16 | post_create_environment: 17 | - doxygen Doxyfile 18 | 19 | # Build documentation in the docs/ directory with mkdocs 20 | mkdocs: 21 | configuration: mkdocs.yml 22 | fail_on_warning: false 23 | 24 | # Optionally declare the Python requirements required to build your docs 25 | python: 26 | install: 27 | - requirements: docs/requirements.txt -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=humble 2 | ARG OS=jammy 3 | ARG FROM_IMAGE=ros:${ROS_DISTRO}-ros-base-${OS} 4 | ARG OVERLAY_WS=/opt/ros/${ROS_DISTRO}/overlay_ws 5 | 6 | FROM ${FROM_IMAGE} AS cacher 7 | 8 | ENV DEBIAN_FRONTEND=noninteractive 9 | RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections 10 | 11 | ENV ROS_DISTRO=${ROS_DISTRO} 12 | ENV REPO_DIR=/root/atos_git 13 | SHELL ["/bin/bash", "-c"] 14 | 15 | WORKDIR ${REPO_DIR} 16 | 17 | COPY ./scripts/installation/ ./scripts/installation/ 18 | COPY ./atos_gui/requirements.txt ./atos_gui/requirements.txt 19 | COPY ./atos_gui/package.xml ./atos_gui/package.xml 20 | COPY ./atos_interfaces/package.xml ./atos_interfaces/package.xml 21 | COPY ./atos/package.xml ./atos/package.xml 22 | 23 | RUN --mount=type=cache,target=/var/cache/apt \ 24 | ./scripts/installation/install_deps.sh ${REPO_DIR} 25 | COPY . . 26 | RUN ./scripts/installation/install_atos.sh ${REPO_DIR} 27 | WORKDIR /root/atos_ws -------------------------------------------------------------------------------- /SECURITY.md: -------------------------------------------------------------------------------- 1 | # Security Policy 2 | 3 | ## Supported Versions 4 | 5 | These version of ATOS are currently being supported with security updates. 6 | 7 | | Version | Supported | 8 | | ------- | ------------------ | 9 | | 1.0.x | :yes: | 10 | 11 | ## Reporting a Vulnerability 12 | 13 | 14 | If you discover a security vulnerability within our project, please open a private vulnerability report on [Github](https://github.com/RI-SE/ATOS/security). All security vulnerabilities will be promptly addressed. 15 | 16 | Please do NOT open an issue on GitHub to report a security vulnerability. Instead, use the above method to get in contact with us directly. 17 | 18 | We appreciate your effort in making our project safer and your patience in the process. 19 | -------------------------------------------------------------------------------- /atos/cmake/cpack/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(CPACK_GENERATOR "DEB") 2 | set(CPACK_DEBIAN_PACKAGE_MAINTAINER "lukas.wikander@astazero.com") 3 | set(CPACK_DEBIAN_PACKAGE_DEPENDS "libeigen3-dev (>=3.0.0), libsystemd-dev, libprotobuf-dev (>=3.0.0)") 4 | set(CPACK_PACKAGE_VENDOR "AstaZero") 5 | set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "AstaZero test track execution software") 6 | set(CPACL_PACKAGE_DESCRIPTION_FILE "CPack.ATOSDescription.txt") 7 | 8 | set(CPACK_DEBIAN_PACKAGE_CONTROL_EXTRA 9 | "${CMAKE_CURRENT_SOURCE_DIR}/postinst" 10 | "${CMAKE_CURRENT_SOURCE_DIR}/postrm" 11 | "${CMAKE_CURRENT_SOURCE_DIR}/prerm") 12 | include(CPack) 13 | -------------------------------------------------------------------------------- /atos/cmake/cpack/CPack.MaestroDescription.txt: -------------------------------------------------------------------------------- 1 | ATOS is a test execution and synchronisation software developed by the AstaZero proving ground. 2 | -------------------------------------------------------------------------------- /atos/cmake/cpack/postinst: -------------------------------------------------------------------------------- 1 | ../../postinstall.sh -------------------------------------------------------------------------------- /atos/cmake/cpack/postrm: -------------------------------------------------------------------------------- 1 | ../../postrm.sh -------------------------------------------------------------------------------- /atos/cmake/cpack/prerm: -------------------------------------------------------------------------------- 1 | ../../prerm.sh -------------------------------------------------------------------------------- /atos/cmake/modules/FindesminiLib.cmake: -------------------------------------------------------------------------------- 1 | # Distributed under the OSI-approved BSD 3-Clause License. See accompanying 2 | # file Copyright.txt or https://cmake.org/licensing for details. 3 | 4 | #[=======================================================================[.rst: 5 | FindesminiLib 6 | ------- 7 | 8 | Finds the esminiLib library. 9 | 10 | Imported Targets 11 | ^^^^^^^^^^^^^^^^ 12 | 13 | This module provides the following imported targets, if found: 14 | 15 | ``esminiLib::esminiLib`` 16 | The esminiLib library 17 | 18 | Result Variables 19 | ^^^^^^^^^^^^^^^^ 20 | 21 | This will define the following variables: 22 | 23 | ``esminiLib_FOUND`` 24 | True if the system has the esminiLib library. 25 | ``esminiLib_VERSION`` 26 | The version of the esminiLib library which was found. 27 | ``esminiLib_INCLUDE_DIRS`` 28 | Include directories needed to use esminiLib. 29 | ``esminiLib_LIBRARIES`` 30 | Libraries needed to link to esminiLib. 31 | 32 | Cache Variables 33 | ^^^^^^^^^^^^^^^ 34 | 35 | The following cache variables may also be set: 36 | 37 | ``esminiLib_INCLUDE_DIR`` 38 | The directory containing ``esminiLib.hpp``, ``esminiRMLib.hpp`` etc.. 39 | ``esminiLib_LIBRARY`` 40 | The path to the esminiLib library. 41 | 42 | #]=======================================================================] 43 | 44 | 45 | find_package(PkgConfig) 46 | pkg_check_modules(PC_esminiLib QUIET esminiLib) 47 | 48 | find_path(esminiLib_INCLUDE_DIR 49 | NAMES esminiLib.hpp 50 | PATHS ${PC_esminiLib_INCLUDE_DIRS} 51 | PATH_SUFFIXES esmini 52 | ) 53 | find_library(esminiLib_LIBRARY 54 | NAMES esminiLib esminiRMLib 55 | PATHS ${PC_esminiLib_LIBRARY_DIRS} 56 | ) 57 | 58 | set(esminiLib_VERSION ${PC_esminiLib_VERSION}) 59 | 60 | include(FindPackageHandleStandardArgs) 61 | find_package_handle_standard_args(esminiLib 62 | FOUND_VAR esminiLib_FOUND 63 | REQUIRED_VARS 64 | esminiLib_LIBRARY 65 | esminiLib_INCLUDE_DIR 66 | VERSION_VAR esminiLib_VERSION 67 | ) 68 | 69 | if(esminiLib_FOUND) 70 | set(esminiLib_LIBRARIES ${esminiLib_LIBRARY}) 71 | set(esminiLib_INCLUDE_DIRS ${esminiLib_INCLUDE_DIR}) 72 | set(esminiLib_DEFINITIONS ${PC_esminiLib_CFLAGS_OTHER}) 73 | endif() 74 | 75 | mark_as_advanced( 76 | esminiLib_INCLUDE_DIR 77 | esminiLib_LIBRARY 78 | ) -------------------------------------------------------------------------------- /atos/cmake/modules/FindesminiRMLib.cmake: -------------------------------------------------------------------------------- 1 | # Distributed under the OSI-approved BSD 3-Clause License. See accompanying 2 | # file Copyright.txt or https://cmake.org/licensing for details. 3 | 4 | #[=======================================================================[.rst: 5 | FindesminiRMLib 6 | ------- 7 | 8 | Finds the esminiRMLib library. 9 | 10 | Imported Targets 11 | ^^^^^^^^^^^^^^^^ 12 | 13 | This module provides the following imported targets, if found: 14 | 15 | ``esminiRMLib::esminiRMLib`` 16 | The esminiRMLib library 17 | 18 | Result Variables 19 | ^^^^^^^^^^^^^^^^ 20 | 21 | This will define the following variables: 22 | 23 | ``esminiRMLib_FOUND`` 24 | True if the system has the esminiRMLib library. 25 | ``esminiRMLib_VERSION`` 26 | The version of the esminiRMLib library which was found. 27 | ``esminiRMLib_INCLUDE_DIRS`` 28 | Include directories needed to use esminiRMLib. 29 | ``esminiRMLib_LIBRARIES`` 30 | Libraries needed to link to esminiRMLib. 31 | 32 | Cache Variables 33 | ^^^^^^^^^^^^^^^ 34 | 35 | The following cache variables may also be set: 36 | 37 | ``esminiRMLib_INCLUDE_DIR`` 38 | The directory containing ``esminiRMLib.hpp``, ``esminiRMRMLib.hpp`` etc.. 39 | ``esminiRMLib_LIBRARY`` 40 | The path to the esminiRMLib library. 41 | 42 | #]=======================================================================] 43 | 44 | 45 | find_package(PkgConfig) 46 | pkg_check_modules(PC_esminiRMLib QUIET esminiRMLib) 47 | 48 | find_path(esminiRMLib_INCLUDE_DIR 49 | NAMES esminiRMLib.hpp 50 | PATHS ${PC_esminiRMLib_INCLUDE_DIRS} 51 | PATH_SUFFIXES esmini 52 | ) 53 | find_library(esminiRMLib_LIBRARY 54 | NAMES esminiRMLib esminiRMRMLib 55 | PATHS ${PC_esminiRMLib_LIBRARY_DIRS} 56 | ) 57 | 58 | set(esminiRMLib_VERSION ${PC_esminiRMLib_VERSION}) 59 | 60 | include(FindPackageHandleStandardArgs) 61 | find_package_handle_standard_args(esminiRMLib 62 | FOUND_VAR esminiRMLib_FOUND 63 | REQUIRED_VARS 64 | esminiRMLib_LIBRARY 65 | esminiRMLib_INCLUDE_DIR 66 | VERSION_VAR esminiRMLib_VERSION 67 | ) 68 | 69 | if(esminiRMLib_FOUND) 70 | set(esminiRMLib_LIBRARIES ${esminiRMLib_LIBRARY}) 71 | set(esminiRMLib_INCLUDE_DIRS ${esminiRMLib_INCLUDE_DIR}) 72 | set(esminiRMLib_DEFINITIONS ${PC_esminiRMLib_CFLAGS_OTHER}) 73 | endif() 74 | 75 | mark_as_advanced( 76 | esminiRMLib_INCLUDE_DIR 77 | esminiRMLib_LIBRARY 78 | ) -------------------------------------------------------------------------------- /atos/common/CRSTransformation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "proj.h" 9 | #include "trajectory.hpp" 10 | #include "rclcpp/logger.hpp" 11 | #include "geometry_msgs/msg/point.hpp" 12 | 13 | class CRSTransformation { 14 | 15 | public: 16 | 17 | CRSTransformation(const std::string &fromCRS, const std::string &toCRS); 18 | void apply(std::vector &traj); 19 | void apply(geometry_msgs::msg::Point &point, PJ_DIRECTION direction); 20 | static std::vector projToLLH(const std::string &projString, const std::string &datum); 21 | static void llhOffsetMeters(double *llh, const double *xyzOffset); 22 | 23 | private: 24 | 25 | std::unique_ptr> ctxt; 26 | std::unique_ptr> projection; 27 | rclcpp::Logger logger = rclcpp::get_logger("CRSTransformation"); 28 | 29 | }; 30 | -------------------------------------------------------------------------------- /atos/common/journal.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #ifndef __JOURNAL_HPP 7 | #define __JOURNAL_HPP 8 | 9 | #define JOURNAL_FILE_ENDING ".jnl" 10 | 11 | #include "util.h" 12 | #include 13 | 14 | typedef enum { 15 | JOURNAL_RECORD_MONITOR_DATA, 16 | JOURNAL_RECORD_EVENT, 17 | JOURNAL_RECORD_STRING 18 | } JournalRecordType; 19 | 20 | int JournalInit(const char* name, rclcpp::Logger logger); 21 | int JournalRecordData(JournalRecordType type, const char* format, ...); 22 | int JournalRecordMonitorData(const ObjectDataType* data); 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /atos/common/loggable.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | #include 8 | 9 | class Loggable { 10 | public: 11 | Loggable(rclcpp::Logger lg) : logger(lg) {} 12 | rclcpp::Logger get_logger() const { 13 | return logger; 14 | } 15 | protected: 16 | rclcpp::Logger logger; 17 | }; -------------------------------------------------------------------------------- /atos/common/module.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "module.hpp" 7 | #include 8 | 9 | using namespace std_msgs::msg; 10 | using namespace std_srvs::srv; 11 | 12 | bool Module::shouldExit(){ 13 | return this->quit; 14 | } 15 | 16 | void Module::onExitMessage(const Empty::SharedPtr){ 17 | this->quit=true; 18 | } 19 | 20 | /*! 21 | * \brief A try/catch wrapper that logs messages 22 | * \param tryExecute function to execute 23 | * \param executeIfFail if executing tryExecute fails, this function is executed 24 | * \param topic The topic to print. 25 | * \param logger The logger to use. 26 | * \return true if the initialization was successful, false otherwise 27 | */ 28 | void Module::tryHandleMessage( 29 | std::function tryExecute, 30 | std::function executeIfFail, 31 | const std::string& topic, 32 | const rclcpp::Logger& logger) 33 | { 34 | try { 35 | RCLCPP_DEBUG(logger, "Handling command on %s", topic.c_str()); 36 | tryExecute(); 37 | } catch (std::invalid_argument& e) { 38 | RCLCPP_ERROR(logger, "Handling command on %s failed - %s", topic.c_str(), e.what()); 39 | executeIfFail(); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /atos/common/osihandler/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | 9 | project(OSIHandler LANGUAGES C CXX) 10 | 11 | set(OSI_HANDLER_LIBRARY_TARGET ${TARGET_NAMESPACE}${PROJECT_NAME}) 12 | 13 | find_package(Protobuf 2.6.1 REQUIRED) 14 | find_package(open_simulation_interface 3.3.1 REQUIRED) 15 | find_package(Eigen3 REQUIRED IMPORTED) 16 | 17 | include(GNUInstallDirs) 18 | 19 | add_library(${OSI_HANDLER_LIBRARY_TARGET} SHARED 20 | ${CMAKE_CURRENT_SOURCE_DIR}/osi_handler.cpp 21 | ) 22 | 23 | # pthread might not be needed depending on your protobuf version. 24 | target_link_libraries(${OSI_HANDLER_LIBRARY_TARGET} LINK_PUBLIC 25 | ${PROTOBUF_LIBRARY} 26 | open_simulation_interface 27 | pthread 28 | ${EIGEN_LIBRARY} 29 | ) 30 | 31 | target_include_directories(${OSI_HANDLER_LIBRARY_TARGET} PUBLIC 32 | $ 33 | $ 34 | ) 35 | set_target_properties(${OSI_HANDLER_LIBRARY_TARGET} PROPERTIES 36 | PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/osi_handler.hpp 37 | 38 | ) 39 | 40 | install(CODE "MESSAGE(STATUS \"Installing target ${OSI_HANDLER_LIBRARY_TARGET}\")") 41 | install(TARGETS ${OSI_HANDLER_LIBRARY_TARGET} 42 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 43 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 44 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 45 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 46 | ) 47 | 48 | -------------------------------------------------------------------------------- /atos/common/osihandler/README.md: -------------------------------------------------------------------------------- 1 | # Install OSI dependencies 2 | 3 | Build OSI and dependencies from source according to [documentation](https://opensimulationinterface.github.io/osi-documentation/index.html#_installing_osi_for_c_on_linux) for Cpp usage. 4 | 5 | Protobuf: 6 | ```bash 7 | sudo apt-get install libprotobuf-dev protobuf-compiler 8 | ``` 9 | 10 | OSI: 11 | ```bash 12 | git clone https://github.com/OpenSimulationInterface/open-simulation-interface.git 13 | cd open-simulation-interface 14 | git checkout 15 | mkdir build 16 | cd build 17 | cmake .. 18 | make 19 | sudo make install 20 | ``` 21 | ## Recommended versions 22 | OSI: == v3.4.0 23 | Protobuf: >=3.0.0 (`protoc --version`) 24 | 25 | OSI v3.5.0 have multiple problems due to new paths where the library is installed. Which requires multiple fixes in multiple cmakelists therefore don't use that version yet. -------------------------------------------------------------------------------- /atos/common/regexpatterns.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #ifndef REGEXPATTERNS_H 7 | #define REGEXPATTERNS_H 8 | #include 9 | 10 | namespace RegexPatterns { 11 | const std::string intPattern = "[0-9]+"; 12 | const std::string floatPattern = "[-+]?[0-9]*\\.?[0-9]+"; 13 | const std::string namePattern = "[a-zA-Z0-9\\._-]+"; 14 | const std::string versionPattern = "v?(" + intPattern + ")\\.?(" + intPattern + ")?\\.?(" + intPattern + ")?"; 15 | } 16 | 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /atos/common/roschannels/cartesiantrajectorychannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include "atos_interfaces/msg/cartesian_trajectory.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace CartesianTrajectory { 13 | const std::string topicName = "cartesian_trajectory"; 14 | using message_type = atos_interfaces::msg::CartesianTrajectory; 15 | 16 | class Pub : public BasePub { 17 | public: 18 | const uint32_t objectId; 19 | Pub(rclcpp::Node& node, const uint32_t id) : 20 | BasePub(node, "object_" + std::to_string(id) + "/" + topicName), 21 | objectId(id) {} 22 | }; 23 | 24 | class Sub : public BaseSub { 25 | public: 26 | const uint32_t objectId; 27 | Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : 28 | BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback), 29 | objectId(id) {} 30 | }; 31 | } 32 | } -------------------------------------------------------------------------------- /atos/common/roschannels/controlsignalchannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include "atos_interfaces/msg/control_signal_percentage.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace ControlSignal { 13 | const std::string topicName = "control_signal"; 14 | using message_type = atos_interfaces::msg::ControlSignalPercentage; 15 | const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); 16 | 17 | class Pub : public BasePub { 18 | public: 19 | Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} 20 | }; 21 | 22 | class Sub : public BaseSub { 23 | public: 24 | Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} 25 | }; 26 | } 27 | } -------------------------------------------------------------------------------- /atos/common/roschannels/customcommandaction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "atos_interfaces/msg/custom_command_action.hpp" 9 | #include "roschannel.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace CustomCommandAction { 13 | const std::string topicName = "custom_command_action"; 14 | using message_type = atos_interfaces::msg::CustomCommandAction; 15 | const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); 16 | 17 | class Pub : public BasePub { 18 | public: 19 | Pub(rclcpp::Node &node, const rclcpp::QoS &qos = defaultQoS) 20 | : BasePub(node, topicName, qos) {} 21 | }; 22 | 23 | class Sub : public BaseSub { 24 | public: 25 | Sub(rclcpp::Node &node, 26 | std::function callback, 27 | const rclcpp::QoS &qos = defaultQoS) 28 | : BaseSub(node, topicName, callback, qos) {} 29 | }; 30 | } // namespace CustomCommandAction 31 | } // namespace ROSChannels -------------------------------------------------------------------------------- /atos/common/roschannels/gnsspathchannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include "foxglove_msgs/msg/geo_json.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace GNSSPath { 13 | const std::string topicName = "gnss_path"; 14 | using message_type = foxglove_msgs::msg::GeoJSON; 15 | 16 | class Pub : public BasePub { 17 | public: 18 | const uint32_t objectId; 19 | Pub(rclcpp::Node& node, const uint32_t id) : 20 | BasePub(node, "object_" + std::to_string(id) + "/" + topicName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), 21 | objectId(id) {} 22 | }; 23 | 24 | class Sub : public BaseSub { 25 | public: 26 | const uint32_t objectId; 27 | Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : 28 | BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), 29 | objectId(id) {} 30 | }; 31 | } 32 | } -------------------------------------------------------------------------------- /atos/common/roschannels/navsatfixchannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include "sensor_msgs/msg/nav_sat_fix.hpp" 10 | #include "monitorchannel.hpp" // TODO: remove this when making translator node that translates monr to various other msg types.. 11 | #include "CRSTransformation.hpp" // TODO: also remove this 12 | 13 | namespace ROSChannels { 14 | namespace NavSatFix { 15 | const std::string topicName = "gnss_fix"; 16 | using message_type = sensor_msgs::msg::NavSatFix; 17 | const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); 18 | 19 | class Pub : public BasePub { 20 | public: 21 | const uint32_t objectId; 22 | Pub(rclcpp::Node& node, const uint32_t id, const rclcpp::QoS& qos = defaultQoS) : 23 | BasePub(node, "object_" + std::to_string(id) + "/" + topicName, qos), 24 | objectId(id) {} 25 | }; 26 | 27 | class Sub : public BaseSub { 28 | public: 29 | const uint32_t objectId; 30 | Sub(rclcpp::Node& node, const uint32_t id, std::function callback, const rclcpp::QoS& qos = defaultQoS) : 31 | BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos), 32 | objectId(id) {} 33 | }; 34 | // TODO: Remove below.. 35 | inline message_type fromROSMonr(std::array origin, const ROSChannels::Monitor::message_type &monr) { 36 | sensor_msgs::msg::NavSatFix msg; 37 | msg.header.stamp = monr.atos_header.header.stamp; 38 | 39 | // Local coordinates to global coordinates 40 | double offset[3] = {monr.pose.pose.position.x, monr.pose.pose.position.y, monr.pose.pose.position.z}; 41 | double llh_0[3] = {origin[0], origin[1], origin[2]}; 42 | CRSTransformation::llhOffsetMeters(llh_0,offset); 43 | msg.header.frame_id = "map"; // TODO 44 | 45 | // Fill in the rest of the message 46 | msg.latitude = llh_0[0]; 47 | msg.longitude = llh_0[1]; 48 | msg.altitude = llh_0[2]; 49 | msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; 50 | return msg; 51 | } 52 | } 53 | } -------------------------------------------------------------------------------- /atos/common/roschannels/objstatechangechannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include "atos_interfaces/msg/object_state_change.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace ObjectStateChange { 13 | const std::string topicName = "object_state_change"; 14 | using message_type = atos_interfaces::msg::ObjectStateChange; 15 | const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); 16 | 17 | class Pub : public BasePub { 18 | public: 19 | Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} 20 | }; 21 | 22 | class Sub : public BaseSub { 23 | public: 24 | Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} 25 | }; 26 | } 27 | } -------------------------------------------------------------------------------- /atos/common/roschannels/pathchannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include "nav_msgs/msg/path.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace Path { 13 | const std::string topicName = "path"; 14 | using message_type = nav_msgs::msg::Path; 15 | 16 | class Pub : public BasePub { 17 | public: 18 | const uint32_t objectId; 19 | Pub(rclcpp::Node& node, const uint32_t id) : 20 | BasePub(node, "object_" + std::to_string(id) + "/" + topicName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), 21 | objectId(id) {} 22 | }; 23 | 24 | class Sub : public BaseSub { 25 | public: 26 | const uint32_t objectId; 27 | Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : 28 | BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), 29 | objectId(id) {} 30 | }; 31 | } 32 | } -------------------------------------------------------------------------------- /atos/common/roschannels/pointcloudchannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include 10 | 11 | namespace ROSChannels { 12 | namespace Pointcloud { 13 | const std::string topicName = "site_scan"; 14 | using message_type = sensor_msgs::msg::PointCloud2; 15 | 16 | class Pub : public BasePub { 17 | public: 18 | const std::string fileName; 19 | Pub(rclcpp::Node& node, const std::string fileName) : 20 | BasePub(node, topicName + "/" + fileName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} 21 | }; 22 | 23 | class Sub : public BaseSub { 24 | public: 25 | const std::string fileName; 26 | Sub(rclcpp::Node& node, const std::string fileName, std::function callback) : 27 | BaseSub(node, topicName + "/" + fileName, callback, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} 28 | }; 29 | } 30 | } -------------------------------------------------------------------------------- /atos/common/roschannels/roschannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace ROSChannels { 13 | 14 | template 15 | class BasePub { 16 | public: 17 | BasePub(rclcpp::Node& node, 18 | const std::string& topicName, 19 | const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) 20 | : pub(node.create_publisher(topicName, qos)) {} 21 | BasePub() = delete; 22 | typename rclcpp::Publisher::SharedPtr pub; 23 | inline virtual void publish(const T& msg) { assert(pub); pub->publish(msg); }; 24 | }; 25 | 26 | template 27 | class BaseSub { 28 | public: 29 | BaseSub(rclcpp::Node& node, 30 | const std::string& topicName, 31 | std::function callback, 32 | const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) 33 | : sub(node.create_subscription(topicName, qos, callback)) {} 34 | BaseSub() = delete; 35 | typename rclcpp::Subscription::SharedPtr sub; 36 | }; 37 | 38 | } // namespace ROSChannels -------------------------------------------------------------------------------- /atos/common/roschannels/scenariochannel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "atos_interfaces/msg/story_board_element_state_change.hpp" 9 | #include "roschannel.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace StoryBoardElementStateChange { 13 | const std::string topicName = "story_board_element_state_change"; 14 | using message_type = atos_interfaces::msg::StoryBoardElementStateChange; 15 | 16 | class Pub : public BasePub { 17 | public: 18 | Pub(rclcpp::Node &node) 19 | : BasePub( 20 | node, topicName, 21 | rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} 22 | }; 23 | 24 | class Sub : public BaseSub { 25 | public: 26 | Sub(rclcpp::Node &node, 27 | std::function callback) 28 | : BaseSub( 29 | node, topicName, callback, 30 | rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} 31 | }; 32 | } // namespace StoryBoardElementStateChange 33 | } // namespace ROSChannels -------------------------------------------------------------------------------- /atos/common/roschannels/statechange.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "roschannel.hpp" 9 | #include "atos_interfaces/msg/state_change.hpp" 10 | 11 | namespace ROSChannels { 12 | namespace StateChange { 13 | const std::string topicName = "state_change"; 14 | using message_type = atos_interfaces::msg::StateChange; 15 | const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); 16 | 17 | class Pub : public BasePub { 18 | public: 19 | Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} 20 | }; 21 | 22 | class Sub : public BaseSub { 23 | public: 24 | Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} 25 | }; 26 | } 27 | } -------------------------------------------------------------------------------- /atos/common/roschannels/test_channels.hpp: -------------------------------------------------------------------------------- 1 | #include "roschannel.hpp" 2 | #include "std_msgs/msg/empty.hpp" 3 | 4 | 5 | namespace ROSChannels { 6 | namespace SampleModuleTestForInitResponse { 7 | const std::string topicName = "sample_module_test_for_init_response"; 8 | using message_type = std_msgs::msg::Empty; 9 | const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); 10 | class Pub : public BasePub { 11 | public: 12 | Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} 13 | }; 14 | 15 | class Sub : public BaseSub { 16 | public: 17 | Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} 18 | }; 19 | } 20 | } -------------------------------------------------------------------------------- /atos/common/shmem/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_C_STANDARD 11) 7 | set(CMAKE_C_STANDARD_REQUIRED ON) 8 | 9 | project(MaestroSHM LANGUAGES C CXX) 10 | 11 | set(SHARED_MEMORY_TARGET ${TARGET_NAMESPACE}${PROJECT_NAME}) 12 | 13 | include(GNUInstallDirs) 14 | 15 | add_library(${SHARED_MEMORY_TARGET} SHARED 16 | ${CMAKE_CURRENT_SOURCE_DIR}/shmem.cpp 17 | ) 18 | 19 | target_include_directories(${SHARED_MEMORY_TARGET} PUBLIC 20 | $ 21 | $ 22 | ) 23 | 24 | set_target_properties(${SHARED_MEMORY_TARGET} PROPERTIES 25 | PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/shmem.h 26 | ) 27 | 28 | target_link_libraries(${SHARED_MEMORY_TARGET} rt pthread) 29 | 30 | install(CODE "MESSAGE(STATUS \"Installing target ${SHARED_MEMORY_TARGET}\")") 31 | install(TARGETS ${SHARED_MEMORY_TARGET} 32 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 33 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 34 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 35 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 36 | ) 37 | -------------------------------------------------------------------------------- /atos/common/shmem/shmem.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #ifndef SHMEM_H 8 | #define SHMEM_H 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | /*! ------------------------------------------------------------------------------ 13 | * -- File : sharedmemory.h 14 | * -- Author : Lukas Wikander 15 | * -- Description : This file provides function for handling shared memory in a 16 | * thread safe manner. 17 | * -- Reference : manpage shm_overview, sem_overview 18 | * ------------------------------------------------------------------------------ 19 | */ 20 | 21 | #include 22 | 23 | volatile void* createSharedMemory(const char* memoryName, const unsigned int initialElements, const size_t elementSize, int* wasCreated); 24 | volatile void* claimSharedMemory(volatile void* addr); 25 | volatile void* releaseSharedMemory(volatile void* addr); 26 | volatile void* resizeSharedMemory(volatile void* addr, const unsigned int newNumberOfElements); 27 | void destroySharedMemory(volatile void* addr); 28 | void closeSharedMemory(volatile void* addr); 29 | int getNumberOfMemoryElements(volatile void* addr); 30 | ssize_t getMemorySize(volatile void* addr); 31 | ssize_t getElementSize(volatile void* addr); 32 | 33 | 34 | 35 | #ifdef __cplusplus 36 | } 37 | #endif 38 | #endif 39 | -------------------------------------------------------------------------------- /atos/common/sockets/canhandler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #include "canhandler.hpp" 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | CANHandler::CANHandler(const bool blocking) { 21 | this->blocking = blocking; 22 | } 23 | 24 | int CANHandler::connectTo( 25 | const std::string &interface) { 26 | 27 | struct ifreq ifr; 28 | can_frame frame; 29 | ssize_t bytesRead = 0; 30 | 31 | if ((sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { 32 | perror("socket"); 33 | return -1; 34 | } 35 | 36 | std::strcpy(ifr.ifr_name, interface.c_str()); 37 | ioctl(sockfd, SIOCGIFINDEX, &ifr); 38 | 39 | addr.can_family = AF_CAN; 40 | addr.can_ifindex = ifr.ifr_ifindex; 41 | 42 | std::cout << "Binding CAN handler to interface " << interface << std::endl; 43 | if (bind(sockfd, reinterpret_cast(&addr), 44 | sizeof (addr))) { 45 | perror("bind"); 46 | return -1; 47 | } 48 | 49 | bytesRead = recv(sockfd, &frame, sizeof (frame), MSG_DONTWAIT); 50 | if (bytesRead < 0 && errno != EAGAIN && errno != EWOULDBLOCK) { 51 | perror("recv"); 52 | close(sockfd); 53 | return -1; 54 | } 55 | 56 | return 0; 57 | } 58 | 59 | ssize_t CANHandler::receive(can_frame &frame) { 60 | ssize_t bytesRead = 0; 61 | bytesRead = recv(sockfd, &frame, sizeof (frame), 62 | blocking ? 0 : MSG_DONTWAIT); 63 | if (bytesRead < 0) { 64 | if (!blocking && (errno == EAGAIN || errno == EWOULDBLOCK)) { 65 | bytesRead = 0; 66 | } 67 | else { 68 | perror("recv"); 69 | close(sockfd); 70 | } 71 | } 72 | return bytesRead; 73 | } 74 | 75 | ssize_t CANHandler::transmit(const can_frame &frame){ 76 | ssize_t bytesSent = 0; 77 | bytesSent = send(sockfd, &frame, sizeof (struct can_frame), 78 | blocking ? 0 : MSG_DONTWAIT); 79 | if (bytesSent < 0) { 80 | if (!blocking && (errno == EAGAIN || errno == EWOULDBLOCK)) { 81 | bytesSent = 0; 82 | } 83 | else { 84 | perror("send"); 85 | close(sockfd); 86 | } 87 | } 88 | return bytesSent; 89 | } 90 | -------------------------------------------------------------------------------- /atos/common/sockets/canhandler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #ifndef CANHANDLER_HPP 8 | #define CANHANDLER_HPP 9 | #include 10 | #include 11 | 12 | class CANHandler { 13 | public: 14 | CANHandler(const bool blocking = true); 15 | void setBlocking(void) { this->blocking = true; } 16 | void setNonblocking(void) { this->blocking = false; } 17 | bool isBlocking() const { return this->blocking; } 18 | int connectTo(const std::string& interface); 19 | ssize_t receive(can_frame &frame); 20 | ssize_t transmit(const can_frame &frame); 21 | private: 22 | int sockfd = 0; 23 | struct sockaddr_can addr = {0}; 24 | bool blocking = true; 25 | }; 26 | 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /atos/common/sockets/client.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #include "client.hpp" 8 | #include 9 | 10 | using namespace SocketErrors; 11 | 12 | Client::Client(const SocketType type, const bool debug) : Socket(type, debug) {} 13 | Client::Client( 14 | const SocketType type, 15 | const Address& remoteAddr, 16 | const Port port, 17 | const bool debug) 18 | : Client(type, debug) { 19 | connect(remoteAddr, port); 20 | } 21 | 22 | TCPClient::TCPClient(const bool debug) : Client(STREAM, debug) {} 23 | TCPClient::TCPClient( 24 | const Address& remoteAddr, 25 | const Port port, 26 | const bool debug) 27 | : Client(STREAM, remoteAddr, port, debug) { 28 | } 29 | 30 | UDPClient::UDPClient(const bool debug) : Client(DATAGRAM, debug) {} 31 | UDPClient::UDPClient( 32 | const Address& remoteAddr, 33 | const Port port, 34 | const bool debug) 35 | : Client(DATAGRAM, remoteAddr, port, debug) { 36 | } 37 | 38 | /*! 39 | * \brief Client::connect Connect the client to a remote host. 40 | * \param remoteAddr Address of the remote host. 41 | * \param port Port on the remote host. 42 | */ 43 | void Client::connect( 44 | const Address& remoteAddr, 45 | const Port port) { 46 | if (remoteAddr.empty() || port == 0) { 47 | throw ArgumentError("Empty address or port specified for connect call"); 48 | } 49 | auto addr = toSockaddr(remoteAddr, port); 50 | if (::connect(mSockfd, reinterpret_cast(&addr), 51 | sizeof (addr)) < 0) { 52 | throw SocketConnectError(errno); 53 | } 54 | } 55 | 56 | -------------------------------------------------------------------------------- /atos/common/sockets/client.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #pragma once 8 | #include "socket.hpp" 9 | class Client : public Socket { 10 | public: 11 | Client(const SocketType type, const bool debug = false); 12 | Client(const SocketType type, const Address& remoteAddr, const Port port, const bool debug = false); 13 | 14 | void connect(); 15 | void connect(const Address&, const Port port); 16 | int disconnect(); 17 | }; 18 | 19 | class TCPClient : public Client { 20 | public: 21 | TCPClient(const bool debug = false); 22 | TCPClient(const Address& remoteAddr, const Port port, const bool debug = false); 23 | }; 24 | 25 | class UDPClient : public Client { 26 | public: 27 | UDPClient(const bool debug = false); 28 | UDPClient(const Address& remoteAddr, const Port port, const bool debug = false); 29 | }; 30 | -------------------------------------------------------------------------------- /atos/common/sockets/server.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #pragma once 8 | #include "socket.hpp" 9 | 10 | class Server : public BasicSocket { 11 | public: 12 | Server(const SocketType type, const bool debug = false); 13 | Server(const SocketType type, const Address& localAddr, const Port port, const bool debug = false); 14 | 15 | protected: 16 | void bind(const SocketType type, const sockaddr_in localAddr); 17 | }; 18 | 19 | class TCPServer : public Server { 20 | public: 21 | TCPServer(const bool debug = false); 22 | TCPServer(const Address& localAddr, const Port port, const bool debug = false); 23 | 24 | virtual Socket await(); 25 | virtual Socket await(const Address& localAddr, const Port port); 26 | protected: 27 | virtual void listen(); 28 | virtual Socket accept(); 29 | private: 30 | static const unsigned int MAX_QUEUED_CONNECTIONS = 10; 31 | }; 32 | 33 | class UDPServer : public Server { 34 | public: 35 | UDPServer(const bool debug = false); 36 | UDPServer(const Address& localAddr, const Port port, const bool debug = false); 37 | 38 | void bind(const HostInfo& localEndpoint); 39 | 40 | std::pair, HostInfo> recvfrom(); 41 | void sendto(const std::pair, const HostInfo>& data); 42 | void sendto(const std::pair, const HostInfo>& data, const size_t nBytes); 43 | private: 44 | static const int IO_BUFFER_SIZE = 4096; 45 | std::vector recvBuffer = std::vector(IO_BUFFER_SIZE); 46 | std::vector sendBuffer = std::vector(IO_BUFFER_SIZE); 47 | }; 48 | -------------------------------------------------------------------------------- /atos/common/sockets/test_can.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #include "canhandler.hpp" 8 | #include 9 | #include 10 | 11 | int main(int argc, char** argv) { 12 | CANHandler ch; 13 | std::string interface = "slcan0"; 14 | std::cout << "Connecting to interface " << interface << std::endl; 15 | assert(ch.connectTo(interface) >= 0); 16 | 17 | // TODO test receive functionality 18 | 19 | can_frame frame; 20 | frame.can_id = 0x123; 21 | frame.data[0] = 0x0B; 22 | frame.data[1] = 0x0E; 23 | frame.data[2] = 0x0E; 24 | frame.data[3] = 0x0F; 25 | frame.can_dlc = 4; 26 | 27 | std::cout << "Sending frame " << frame.can_id << " on CAN" << std::endl; 28 | ssize_t bytesSent = ch.transmit(frame); 29 | assert(bytesSent > 0); // TODO exact byte count 30 | std::cout << "Sent " << bytesSent << " bytes" << std::endl; 31 | 32 | frame.can_id = 0x100; 33 | frame.data[0] = 0x0D; 34 | frame.data[1] = 0x0E; 35 | frame.data[2] = 0x0A; 36 | frame.data[3] = 0x0D; 37 | frame.data[4] = 0x0B; 38 | frame.data[5] = 0x0E; 39 | frame.data[6] = 0x0E; 40 | frame.data[7] = 0x0F; 41 | frame.can_dlc = 8; 42 | 43 | std::cout << "Sending frame " << frame.can_id << " on CAN" << std::endl; 44 | bytesSent = ch.transmit(frame); 45 | assert(bytesSent > 0); // TODO exact byte count 46 | std::cout << "Sent " << bytesSent << " bytes" << std::endl; 47 | std::cout << "CAN handler test successful" << std::endl; 48 | return 0; 49 | 50 | } 51 | -------------------------------------------------------------------------------- /atos/common/time/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_C_STANDARD 11) 7 | set(CMAKE_C_STANDARD_REQUIRED ON) 8 | 9 | project(ATOSTime LANGUAGES C) 10 | 11 | set(TIME_TARGET ${TARGET_NAMESPACE}${PROJECT_NAME}) 12 | 13 | include(GNUInstallDirs) 14 | 15 | add_library(${TIME_TARGET} SHARED 16 | ${CMAKE_CURRENT_SOURCE_DIR}/atosTime.c 17 | ) 18 | 19 | target_include_directories(${TIME_TARGET} PUBLIC 20 | $ 21 | $ 22 | ) 23 | 24 | set_target_properties(${TIME_TARGET} PROPERTIES 25 | PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/atosTime.h 26 | ) 27 | 28 | install(CODE "MESSAGE(STATUS \"Installing target ${TIME_TARGET}\")") 29 | install(TARGETS ${TIME_TARGET} 30 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 31 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 32 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 33 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 34 | ) 35 | -------------------------------------------------------------------------------- /atos/common/type.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #include "type.h" 8 | #ifdef __GNUG__ 9 | #include 10 | #include 11 | #include 12 | 13 | std::string demangle(const char* name) { 14 | 15 | int status = -4; // some arbitrary value to eliminate the compiler warning 16 | 17 | std::unique_ptr res { 18 | abi::__cxa_demangle(name, NULL, NULL, &status), 19 | std::free 20 | }; 21 | 22 | return (status==0) ? res.get() : name ; 23 | } 24 | 25 | #else 26 | 27 | // does nothing if not g++ 28 | std::string demangle(const char* name) { 29 | return name; 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /atos/common/type.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #ifndef TYPE_H 7 | #define TYPE_H 8 | #ifdef __cplusplus 9 | #include 10 | #include 11 | 12 | std::string demangle(const char* name); 13 | 14 | /*! 15 | * \brief type returns a string based on the typename of T 16 | * \return A string representation of the typename 17 | */ 18 | template 19 | std::string type(const T& t) { 20 | 21 | return demangle(typeid(t).name()); 22 | } 23 | 24 | #endif 25 | #endif -------------------------------------------------------------------------------- /atos/launch/launch_basic.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | from ament_index_python.packages import get_package_prefix 4 | sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path 5 | get_package_prefix('atos'), 6 | 'share', 'atos', 'launch')) 7 | import launch_utils.launch_base as launch_base 8 | from launch import LaunchDescription 9 | 10 | def generate_launch_description(): 11 | base_nodes = launch_base.get_base_nodes() 12 | return LaunchDescription(base_nodes) 13 | -------------------------------------------------------------------------------- /atos/launch/launch_full.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | from ament_index_python.packages import get_package_prefix 4 | sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path 5 | get_package_prefix('atos'), 6 | 'share', 'atos', 'launch')) 7 | import launch_utils.launch_base as launch_base 8 | from launch import LaunchDescription 9 | from launch_ros.actions import Node 10 | from launch_graphical import get_graphical_nodes 11 | from launch_experimental import get_experimental_nodes 12 | 13 | 14 | def generate_launch_description(): 15 | files = launch_base.get_files() 16 | base_nodes = launch_base.get_base_nodes() 17 | graphical_nodes = get_graphical_nodes() 18 | experimental_nodes = get_experimental_nodes() 19 | 20 | for node in graphical_nodes: 21 | base_nodes.append(node) 22 | 23 | for node in experimental_nodes: 24 | base_nodes.append(node) 25 | 26 | return LaunchDescription(base_nodes) 27 | -------------------------------------------------------------------------------- /atos/launch/launch_graphical.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | from ament_index_python.packages import get_package_prefix 4 | sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path 5 | get_package_prefix('atos'), 6 | 'share', 'atos', 'launch')) 7 | import launch_utils.launch_base as launch_base 8 | from launch import LaunchDescription 9 | from launch_ros.actions import Node 10 | 11 | def get_graphical_nodes(): 12 | files = launch_base.get_files() 13 | return [ 14 | Node( 15 | package='atos', 16 | namespace='atos', 17 | executable='pointcloud_publisher', 18 | name='pointcloud_publisher', 19 | parameters=[files["params"]] 20 | ) 21 | ] 22 | 23 | def generate_launch_description(): 24 | base_nodes = launch_base.get_base_nodes() 25 | graphical_nodes = get_graphical_nodes() 26 | 27 | for node in graphical_nodes: 28 | base_nodes.append(node) 29 | 30 | return LaunchDescription(base_nodes) 31 | -------------------------------------------------------------------------------- /atos/launch/launch_integration_testing.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | from ament_index_python.packages import get_package_prefix 4 | sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path 5 | get_package_prefix('atos'), 6 | 'share', 'atos', 'launch')) 7 | from launch_ros.actions import Node 8 | from launch import LaunchDescription 9 | import launch_utils.launch_base as launch_base 10 | 11 | 12 | def get_integration_test_nodes(): 13 | files = launch_base.get_files() 14 | return [ 15 | Node( 16 | package='atos', 17 | namespace='atos', 18 | executable='integration_testing_handler', 19 | parameters=[files["params"]] 20 | ) 21 | ] 22 | 23 | def generate_launch_description(): 24 | base_nodes = launch_base.get_base_nodes() 25 | 26 | integration_test_nodes = get_integration_test_nodes() 27 | 28 | for node in integration_test_nodes: 29 | base_nodes.append(node) 30 | 31 | return LaunchDescription(base_nodes) 32 | -------------------------------------------------------------------------------- /atos/launch/launch_utils/generate_cert.py: -------------------------------------------------------------------------------- 1 | from OpenSSL import crypto 2 | def cert_gen( 3 | emailAddress="emailAddress", 4 | commonName="commonName", 5 | countryName="NT", 6 | localityName="localityName", 7 | stateOrProvinceName="stateOrProvinceName", 8 | organizationName="organizationName", 9 | organizationUnitName="organizationUnitName", 10 | serialNumber=0, 11 | validityStartInSeconds=0, 12 | validityEndInSeconds=10*365*24*60*60, 13 | KEY_FILE = "private.key", 14 | CERT_FILE="selfsigned.crt"): 15 | #can look at generated file using openssl: 16 | #openssl x509 -inform pem -in selfsigned.crt -noout -text 17 | # create a key pair 18 | k = crypto.PKey() 19 | k.generate_key(crypto.TYPE_RSA, 4096) 20 | # create a self-signed cert 21 | cert = crypto.X509() 22 | cert.get_subject().C = countryName 23 | cert.get_subject().ST = stateOrProvinceName 24 | cert.get_subject().L = localityName 25 | cert.get_subject().O = organizationName 26 | cert.get_subject().OU = organizationUnitName 27 | cert.get_subject().CN = commonName 28 | cert.get_subject().emailAddress = emailAddress 29 | cert.set_serial_number(serialNumber) 30 | cert.gmtime_adj_notBefore(0) 31 | cert.gmtime_adj_notAfter(validityEndInSeconds) 32 | cert.set_issuer(cert.get_subject()) 33 | cert.set_pubkey(k) 34 | cert.sign(k, 'sha512') 35 | with open(CERT_FILE, "wt") as f: 36 | f.write(crypto.dump_certificate(crypto.FILETYPE_PEM, cert).decode("utf-8")) 37 | with open(KEY_FILE, "wt") as f: 38 | f.write(crypto.dump_privatekey(crypto.FILETYPE_PEM, k).decode("utf-8")) 39 | -------------------------------------------------------------------------------- /atos/modules/BackToStart/inc/backtostart.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "module.hpp" 9 | #include "trajectory.hpp" 10 | #include "atos_interfaces/srv/get_object_return_trajectory.hpp" 11 | 12 | /*! 13 | * \brief The BackToStart class offers services to calculate a trajectory to return test objects to start position. 14 | */ 15 | class BackToStart : public Module { 16 | public: 17 | BackToStart(); 18 | 19 | private: 20 | static inline std::string const moduleName = "back_to_start"; 21 | 22 | rclcpp::Service::SharedPtr getObjectReturnTrajectoryService; //!< Service to request object return trajectory 23 | 24 | void onReturnTrajectoryRequest(const std::shared_ptr, 25 | std::shared_ptr); 26 | }; 27 | -------------------------------------------------------------------------------- /atos/modules/BackToStart/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "backtostart.hpp" 7 | 8 | 9 | int main(int argc, char** argv){ 10 | rclcpp::init(argc, argv); 11 | std::shared_ptr btsNode = std::make_shared(); 12 | rclcpp::spin(btsNode); 13 | rclcpp::shutdown(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /atos/modules/BackToStart/tests/main.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | int main(int argc, char** argv) 4 | { 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } -------------------------------------------------------------------------------- /atos/modules/DirectControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.8) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_C_STANDARD 11) 9 | set(CMAKE_C_STANDARD_REQUIRED ON) 10 | 11 | project(direct_control) 12 | find_package(atos_interfaces REQUIRED) 13 | 14 | # Define target names 15 | set(DIRECT_CONTROL_TARGET ${PROJECT_NAME}) 16 | 17 | set(COREUTILS_LIBRARY ATOSCoreUtil) 18 | set(ISO_22133_LIBRARY MaestroISO22133) 19 | set(POSITIONING_LIBRARY MaestroPositioning) 20 | set(TIME_LIBRARY ATOSTime) 21 | set(COMMON_LIBRARY ATOSCommon) 22 | set(SOCKET_LIBRARY TCPUDPSocket) 23 | set(THREAD_LIBRARY pthread) 24 | 25 | get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES) 26 | 27 | include(GNUInstallDirs) 28 | 29 | # Create project main executable target 30 | add_executable(${DIRECT_CONTROL_TARGET} 31 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 32 | ${CMAKE_CURRENT_SOURCE_DIR}/src/directcontrol.cpp 33 | ) 34 | # Link project executable to util libraries 35 | target_link_libraries(${DIRECT_CONTROL_TARGET} 36 | ${TIME_LIBRARY} 37 | ${COREUTILS_LIBRARY} 38 | ${LOGGING_LIBRARY} 39 | ${SOCKET_LIBRARY} 40 | ${THREAD_LIBRARY} 41 | ${COMMON_LIBRARY} 42 | ) 43 | 44 | target_include_directories(${DIRECT_CONTROL_TARGET} PUBLIC SYSTEM 45 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 46 | ${COMMON_HEADERS} 47 | ) 48 | 49 | # ROS specific rules 50 | ament_target_dependencies(${DIRECT_CONTROL_TARGET} 51 | rclcpp 52 | std_msgs 53 | atos_interfaces 54 | ) 55 | 56 | # Installation rules 57 | install(CODE "MESSAGE(STATUS \"Installing target ${DIRECT_CONTROL_TARGET}\")") 58 | install(TARGETS ${DIRECT_CONTROL_TARGET} 59 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 60 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 61 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 62 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 63 | ) 64 | -------------------------------------------------------------------------------- /atos/modules/DirectControl/inc/directcontrol.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include 9 | #include "module.hpp" 10 | #include "tcphandler.hpp" 11 | #include "server.hpp" 12 | #include "atos_interfaces/msg/control_signal_percentage.hpp" 13 | 14 | class DirectControl : public Module { 15 | public: 16 | static inline std::string const moduleName = "direct_control"; 17 | DirectControl(); 18 | int initializeModule(); 19 | void startThreads(); 20 | void joinThreads(); 21 | 22 | private: 23 | static inline const int TCPPort = 53260; 24 | static inline const int UDPPort = 53995; 25 | 26 | void readTCPSocketData(); 27 | void readUDPSocketData(); 28 | void handleISOMessage(std::vector& byteData, size_t receivedBytes); 29 | size_t handleRDCAMessage(std::vector& byteData); 30 | size_t handleUnknownMessage(std::vector& byteData); 31 | ROSChannels::ControlSignal::Pub controlSignalPub; 32 | 33 | void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) override; 34 | void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr) override; 35 | 36 | std::unique_ptr receiveThread; 37 | std::unique_ptr receiveThreadUDP; 38 | volatile bool quit = false; 39 | TCPHandler tcpHandler; 40 | UDPServer udpServer; 41 | }; 42 | -------------------------------------------------------------------------------- /atos/modules/DirectControl/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include 7 | #include 8 | #include 9 | 10 | #include "util.h" 11 | #include "directcontrol.hpp" 12 | 13 | using namespace std::chrono; 14 | 15 | static std::shared_ptr dc; 16 | 17 | int main(int argc, char** argv) { 18 | rclcpp::init(argc,argv); 19 | dc = std::make_shared(); 20 | dc->initializeModule(); 21 | dc->startThreads(); 22 | rclcpp::spin(dc); 23 | dc->joinThreads(); 24 | rclcpp::shutdown(); 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /atos/modules/EsminiAdapter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.8) 6 | 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_C_STANDARD 11) 10 | set(CMAKE_C_STANDARD_REQUIRED ON) 11 | 12 | project(esmini_adapter) 13 | find_package(atos_interfaces REQUIRED) 14 | find_package(tf2 REQUIRED) 15 | find_package(esminiLib REQUIRED) 16 | find_package(esminiRMLib REQUIRED) 17 | find_package(foxglove_msgs REQUIRED) 18 | 19 | # Define target names 20 | set(ESMINI_ADAPTER_TARGET ${PROJECT_NAME}) 21 | 22 | set(ATOS_COMMON_LIBRARY ATOSCommon) 23 | set(COREUTILS_LIBRARY ATOSCoreUtil) 24 | get_target_property(COMMON_HEADERS ${ATOS_COMMON_LIBRARY} INCLUDE_DIRECTORIES) 25 | include(GNUInstallDirs) 26 | 27 | # Create project main executable target 28 | add_executable(${ESMINI_ADAPTER_TARGET} 29 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 30 | ${CMAKE_CURRENT_SOURCE_DIR}/src/esminiadapter.cpp 31 | ) 32 | # Link project executable to util libraries 33 | target_link_libraries(${ESMINI_ADAPTER_TARGET} 34 | ${COREUTILS_LIBRARY} 35 | ${ATOS_COMMON_LIBRARY} 36 | ${esminiLib_LIBRARIES} 37 | ${esminiRMLib_LIBRARIES} 38 | ) 39 | 40 | target_include_directories(${ESMINI_ADAPTER_TARGET} PUBLIC SYSTEM 41 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 42 | ${COMMON_HEADERS} 43 | ${esminiLib_INCLUDE_DIRS} 44 | ${esminiRMLib_INCLUDE_DIRS} 45 | ) 46 | 47 | # ROS specific rules 48 | ament_target_dependencies(${ESMINI_ADAPTER_TARGET} 49 | rclcpp 50 | std_msgs 51 | atos_interfaces 52 | foxglove_msgs 53 | tf2 54 | ) 55 | 56 | # Installation rules 57 | install(CODE "MESSAGE(STATUS \"Installing target ${ESMINI_ADAPTER_TARGET}\")") 58 | install(TARGETS ${ESMINI_ADAPTER_TARGET} 59 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 60 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 61 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 62 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 63 | ) 64 | -------------------------------------------------------------------------------- /atos/modules/EsminiAdapter/inc/string_utility.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | 11 | // Function to split a string into a vector of strings by a delimiter 12 | void split (const std::string &s, char delim, std::vector &elems) { 13 | std::stringstream ss(s); 14 | std::string item; 15 | while (std::getline(ss, item, delim)) { 16 | elems.push_back(item); 17 | } 18 | } -------------------------------------------------------------------------------- /atos/modules/EsminiAdapter/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "esminiadapter.hpp" 7 | 8 | 9 | static std::shared_ptr esminiAdapter; 10 | 11 | int main(int argc, char** argv) { 12 | rclcpp::init(argc,argv); 13 | auto exec = std::make_shared(); 14 | esminiAdapter = EsminiAdapter::instance(); 15 | esminiAdapter->initializeModule(); 16 | exec->add_node(esminiAdapter); 17 | exec->spin(); 18 | rclcpp::shutdown(); 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_C_STANDARD 11) 9 | set(CMAKE_C_STANDARD_REQUIRED ON) 10 | 11 | project(integration_testing_handler) 12 | 13 | find_package(rclcpp REQUIRED) 14 | find_package(std_msgs REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(atos_interfaces REQUIRED) 17 | 18 | # Define target names 19 | set(INTEGRATION_TESTING_TARGET ${PROJECT_NAME}) 20 | 21 | set(ATOS_COMMON_LIBRARY ATOSCommon) 22 | get_target_property(COMMON_HEADERS ${ATOS_COMMON_LIBRARY} INCLUDE_DIRECTORIES) 23 | 24 | include(GNUInstallDirs) 25 | 26 | # Create project main executable target 27 | add_executable(${INTEGRATION_TESTING_TARGET} 28 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 29 | ${CMAKE_CURRENT_SOURCE_DIR}/src/integrationtestinghandler.cpp 30 | ${CMAKE_CURRENT_SOURCE_DIR}/src/integrationtestingfactory.cpp 31 | ${CMAKE_CURRENT_SOURCE_DIR}/src/integrationtesting.cpp 32 | ${CMAKE_CURRENT_SOURCE_DIR}/src/scenarioexecution.cpp 33 | ) 34 | # Link project executable to util libraries 35 | target_link_libraries(${INTEGRATION_TESTING_TARGET} 36 | ${ATOS_COMMON_LIBRARY} 37 | ) 38 | 39 | # ROS specific settings 40 | ament_target_dependencies(${INTEGRATION_TESTING_TARGET} 41 | rclcpp 42 | std_msgs 43 | sensor_msgs 44 | atos_interfaces 45 | ) 46 | 47 | target_include_directories(${INTEGRATION_TESTING_TARGET} PUBLIC 48 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 49 | ${COMMON_HEADERS} 50 | ) 51 | 52 | # Installation rules 53 | install(CODE "MESSAGE(STATUS \"Installing target ${OSI_ADAPTER_TARGET}\")") 54 | install(TARGETS ${INTEGRATION_TESTING_TARGET} 55 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 56 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 57 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 58 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 59 | ) -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/inc/integrationtesting.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "module.hpp" 9 | #include "roschannels/commandchannels.hpp" 10 | #include "atos_interfaces/srv/get_object_control_state.hpp" 11 | 12 | 13 | class IntegrationTesting : public Module { 14 | 15 | public: 16 | IntegrationTesting(const std::string& moduleName); 17 | ~IntegrationTesting(); 18 | 19 | static inline std::string const testName = "scenario_execution"; 20 | virtual void runIntegrationTest() = 0; 21 | 22 | protected: 23 | const std::string atosNamespace = "/atos/"; 24 | const std::string initTopic = atosNamespace + ROSChannels::Init::topicName; 25 | const std::string connectTopic = atosNamespace + ROSChannels::Connect::topicName; 26 | const std::string armTopic = atosNamespace + ROSChannels::Arm::topicName; 27 | const std::string startTopic = atosNamespace + ROSChannels::Start::topicName; 28 | std::shared_ptr> initPub; 29 | std::shared_ptr> connectPub; 30 | std::shared_ptr> armPub; 31 | std::shared_ptr> startPub; 32 | std::shared_ptr> getObjectControlStateClient; 33 | std::vector> stateResult; 34 | 35 | virtual void printResult() = 0; 36 | int getObjectControlState(); 37 | void checkState(const std::string& command); 38 | }; -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/inc/integrationtestingfactory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "integrationtesting.hpp" 9 | #include "scenarioexecution.hpp" 10 | 11 | 12 | class IntegrationTestingFactory { 13 | 14 | public: 15 | IntegrationTestingFactory(); 16 | ~IntegrationTestingFactory(); 17 | 18 | std::shared_ptr createIntegrationTestExecution(const std::string& testName); 19 | }; -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/inc/integrationtestinghandler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "module.hpp" 9 | #include "roschannels/commandchannels.hpp" 10 | 11 | 12 | class IntegrationTestingHandler : public Module { 13 | 14 | public: 15 | IntegrationTestingHandler(); 16 | ~IntegrationTestingHandler(); 17 | 18 | private: 19 | static inline std::string const moduleName = "integration_testing_handler"; 20 | std::map integrationTests; 21 | 22 | void getIntegrationTests(); 23 | void executeIntegrationTests(); 24 | }; 25 | -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/inc/scenarioexecution.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "integrationtesting.hpp" 9 | #include "atos_interfaces/msg/monitor.hpp" 10 | #include "atos_interfaces/srv/get_object_trajectory.hpp" 11 | 12 | 13 | class ScenarioExecution : public IntegrationTesting { 14 | 15 | public: 16 | ScenarioExecution(); 17 | ~ScenarioExecution(); 18 | 19 | private: 20 | std::shared_ptr> monitorSub; 21 | std::shared_ptr> getObjectTrajectoryClient; 22 | bool followedTrajectory; 23 | 24 | void runIntegrationTest() override; 25 | void printResult() override; 26 | std::vector> getTrajectoryPoints(); 27 | void checkObjectStoppedAtLastPoint(); 28 | void placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg); 29 | }; 30 | -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/src/integrationtestingfactory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "integrationtestingfactory.hpp" 7 | 8 | /** 9 | * @brief Factory class for creating integration tests. 10 | * 11 | */ 12 | IntegrationTestingFactory::IntegrationTestingFactory() {} 13 | 14 | 15 | /** 16 | * @brief Destructor. 17 | * 18 | */ 19 | IntegrationTestingFactory::~IntegrationTestingFactory() {} 20 | 21 | 22 | /** 23 | * @brief Create an integration test object. 24 | * 25 | * @param testName Which integration test to create. 26 | * @return std::shared_ptr New integration test object. 27 | */ 28 | std::shared_ptr IntegrationTestingFactory::createIntegrationTestExecution(const std::string& testName) { 29 | if (testName == ScenarioExecution::testName) { 30 | return std::make_shared(); 31 | } 32 | else { 33 | throw std::runtime_error("Unknown integration test: " + testName); 34 | } 35 | } -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/src/integrationtestinghandler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "integrationtestingfactory.hpp" 7 | #include "integrationtestinghandler.hpp" 8 | #include "scenarioexecution.hpp" 9 | 10 | 11 | /** 12 | * @brief Class for creating integration tests. The idea is to set which integrations tests to run in 13 | * params.yaml, and this class will read the params and create the integration tests. 14 | * 15 | */ 16 | IntegrationTestingHandler::IntegrationTestingHandler() : Module(moduleName) { 17 | getIntegrationTests(); 18 | executeIntegrationTests(); 19 | } 20 | 21 | /** 22 | * @brief Destructor. 23 | * 24 | */ 25 | IntegrationTestingHandler::~IntegrationTestingHandler() {} 26 | 27 | 28 | /** 29 | * @brief Get the integration tests to run from params.yaml. 30 | * 31 | */ 32 | void IntegrationTestingHandler::getIntegrationTests() { 33 | declare_parameter("scenario_execution", false); 34 | get_parameter("scenario_execution", integrationTests["scenario_execution"]); 35 | } 36 | 37 | 38 | /** 39 | * @brief Execute the selected integration tests. Each integration test is run in a separate node. 40 | * 41 | */ 42 | void IntegrationTestingHandler::executeIntegrationTests() { 43 | std::this_thread::sleep_for(std::chrono::seconds(3)); // sleep thread to allow other nodes to start 44 | 45 | for (auto const& [testName, testEnabled] : integrationTests) { 46 | if (testEnabled) { 47 | auto executor = std::make_shared(); 48 | auto integrationTestFactory = std::make_shared(); 49 | auto integrationTest = integrationTestFactory->createIntegrationTestExecution(testName); 50 | executor->add_node(integrationTest); 51 | executor->spin_once(); 52 | } 53 | } 54 | RCLCPP_INFO(get_logger(), "Finished executing integration tests"); 55 | } -------------------------------------------------------------------------------- /atos/modules/IntegrationTesting/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "integrationtestinghandler.hpp" 7 | 8 | int main(int argc, char **argv) { 9 | rclcpp::init(argc, argv); 10 | auto integrationTestingHandlerNode = std::make_shared(); 11 | rclcpp::spin(integrationTestingHandlerNode); 12 | rclcpp::shutdown(); 13 | return 0; 14 | } -------------------------------------------------------------------------------- /atos/modules/JournalControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_C_STANDARD 11) 9 | set(CMAKE_C_STANDARD_REQUIRED ON) 10 | 11 | project(journal_control) 12 | 13 | find_package(rclcpp REQUIRED) 14 | find_package(atos_interfaces REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | 17 | # Define target names 18 | set(JOURNAL_CONTROL_TARGET ${PROJECT_NAME}) 19 | 20 | set(COREUTILS_LIBRARY ATOSCoreUtil) 21 | set(FILESYSTEM_LIBRARY stdc++fs) 22 | set(COMMON_LIBRARY ATOSCommon) 23 | 24 | get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES) 25 | get_target_property(COREUTILS_HEADERS ${COREUTILS_LIBRARY} INCLUDE_DIRECTORIES) 26 | 27 | include(GNUInstallDirs) 28 | 29 | # Create project main executable target 30 | add_executable(${JOURNAL_CONTROL_TARGET} 31 | ${CMAKE_CURRENT_SOURCE_DIR}/src/journalmodelcollection.cpp 32 | ${CMAKE_CURRENT_SOURCE_DIR}/src/journalcontrol.cpp 33 | ${CMAKE_CURRENT_SOURCE_DIR}/src/journalmodel.cpp 34 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 35 | ) 36 | # Link project executable to util libraries 37 | target_link_libraries(${JOURNAL_CONTROL_TARGET} 38 | ${rclcpp_LIBRARIES} 39 | ${FILESYSTEM_LIBRARY} 40 | ${COREUTILS_LIBRARY} 41 | ${COMMON_LIBRARY} 42 | ) 43 | target_include_directories(${JOURNAL_CONTROL_TARGET} PUBLIC SYSTEM 44 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 45 | ${COREUTILS_HEADERS} 46 | ${COMMON_HEADERS} 47 | ) 48 | 49 | # ROS specific rules 50 | ament_target_dependencies(${JOURNAL_CONTROL_TARGET} 51 | rclcpp 52 | std_msgs 53 | atos_interfaces 54 | ) 55 | 56 | # Installation rules 57 | install(CODE "MESSAGE(STATUS \"Installing target ${JOURNAL_CONTROL_TARGET}\")") 58 | install(TARGETS ${JOURNAL_CONTROL_TARGET} 59 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 60 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 61 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 62 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 63 | ) 64 | 65 | -------------------------------------------------------------------------------- /atos/modules/JournalControl/inc/journalcontrol.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | /*------------------------------------------------------------------------------ 7 | -- File : journalcontrol.h 8 | -- Author : Lukas Wikander 9 | -- Description : 10 | -- Purpose : 11 | -- Reference : 12 | ------------------------------------------------------------------------------*/ 13 | 14 | #ifndef __JOURNALCONTROL_H_INCLUDED__ 15 | #define __JOURNALCONTROL_H_INCLUDED__ 16 | 17 | #ifdef __cplusplus 18 | extern "C" { 19 | #endif 20 | 21 | #include "util.h" 22 | 23 | #ifdef __cplusplus 24 | } 25 | #endif 26 | 27 | 28 | #include "module.hpp" 29 | #include "journalmodelcollection.hpp" 30 | /*------------------------------------------------------------ 31 | -- Function declarations. 32 | ------------------------------------------------------------*/ 33 | class JournalControl : public Module 34 | { 35 | public: 36 | explicit JournalControl(); 37 | void initialize(); 38 | private: 39 | static inline std::string const moduleName = "journal_control"; 40 | std::string scenarioName; 41 | 42 | JournalModelCollection journals; 43 | 44 | void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr) override; 45 | void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) override; 46 | void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) override; 47 | void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr) override; 48 | 49 | ROSChannels::Arm::Sub armSub; 50 | ROSChannels::Stop::Sub stopSub; 51 | ROSChannels::Abort::Sub abortSub; 52 | ROSChannels::Replay::Sub replaySub; 53 | ROSChannels::Exit::Sub exitSub; 54 | }; 55 | 56 | #endif //__LOGGER_H_INCLUDED__ 57 | -------------------------------------------------------------------------------- /atos/modules/JournalControl/inc/journalmodel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #ifndef JOURNALMODEL_HPP 7 | #define JOURNALMODEL_HPP 8 | 9 | #if __GNUC__ > 8 || (__GNUC__ == 8 && __GNUC_MINOR__ >= 1) 10 | #include 11 | namespace fs = std::filesystem; 12 | #else 13 | #include 14 | namespace fs = std::experimental::filesystem; 15 | #endif 16 | 17 | #include 18 | #include "loggable.hpp" 19 | 20 | class JournalModel : public Loggable { 21 | public: 22 | class Bookmark : public Loggable { 23 | private: 24 | fs::path filePath; 25 | std::streampos filePosition; 26 | public: 27 | Bookmark(rclcpp::Logger log) : Loggable(log) {} 28 | bool valid = false; 29 | void place(const fs::path &path, const bool placeAtBeginning=false); 30 | const std::streampos& getPosition() const { return filePosition; } 31 | const fs::path& getFilePath() const { return filePath; } 32 | std::string toString() const { 33 | return valid ? getFilePath().string() + " @" + std::to_string(getPosition()) 34 | : ""; 35 | } 36 | }; 37 | 38 | JournalModel(rclcpp::Logger log) : Loggable(log), startReference(log), stopReference(log) {} 39 | std::string moduleName; 40 | mutable Bookmark startReference; 41 | mutable Bookmark stopReference; 42 | mutable std::set containedFiles; 43 | 44 | bool operator==(const JournalModel &other) const { 45 | return this->moduleName == other.moduleName; 46 | } 47 | 48 | std::string toString() const; 49 | }; 50 | 51 | //! Template specialization of std::hash for JournalModel 52 | namespace std { 53 | template <> struct hash { 54 | size_t operator() (const JournalModel &journal) const { 55 | return std::hash{}(journal.moduleName); 56 | } 57 | }; 58 | } 59 | 60 | #endif // JOURNALMODEL_HPP -------------------------------------------------------------------------------- /atos/modules/JournalControl/inc/journalmodelcollection.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #ifndef JOURNALMODELCOLLECTION_HPP 7 | #define JOURNALMODELCOLLECTION_HPP 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "journalmodel.hpp" 14 | #include "loggable.hpp" 15 | 16 | // Add a C++20 type 17 | namespace std::chrono { 18 | typedef duration> days; 19 | } 20 | 21 | class JournalModelCollection : public std::unordered_set, public Loggable { 22 | public: 23 | JournalModelCollection(rclcpp::Logger log) : Loggable(log) {} 24 | void placeStartBookmarks(); 25 | void placeStopBookmarks(); 26 | void insertNonBookmarked(); 27 | int dumpToFile(std::string filename); 28 | std::string toString() const { 29 | std::string retval = ""; 30 | for (const auto &journal : *this) { 31 | retval += journal.toString() + "\n"; 32 | } 33 | if (this->size() > 0) { 34 | retval.pop_back(); 35 | } 36 | return retval; 37 | } 38 | private: 39 | std::chrono::time_point startDay; 40 | std::chrono::time_point stopDay; 41 | 42 | 43 | static std::string getDateAsString(const std::chrono::system_clock::time_point &date); 44 | static std::string getCurrentDateAsString(); 45 | static std::vector getJournalFilesFrom(const std::chrono::system_clock::time_point &date); 46 | static std::vector getJournalFilesFromToday() { 47 | return getJournalFilesFrom(std::chrono::system_clock::now()); 48 | } 49 | static int printFilesTo(const fs::path &inputDirectory, std::ostream &outputFile); 50 | }; 51 | 52 | #endif // JOURNALMODELCOLLECTION_HPP -------------------------------------------------------------------------------- /atos/modules/JournalControl/src/journalmodel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "journalmodel.hpp" 7 | #include 8 | #include 9 | 10 | std::string JournalModel::toString() const { 11 | std::string retval = ""; 12 | retval += moduleName + " journal\n\tFiles:\n"; 13 | for (const auto &file : containedFiles) { 14 | retval += "\t\t* " + file.string() + "\n"; 15 | } 16 | retval += "\tStart / end references:\n"; 17 | retval += "\t\ts: " + startReference.toString() + "\n"; 18 | retval += "\t\te: " + stopReference.toString(); 19 | return retval; 20 | } 21 | 22 | void JournalModel::Bookmark::place(const fs::path &path, const bool placeAtBeginning) { 23 | std::ifstream istrm(path, placeAtBeginning ? std::ios_base::in 24 | : std::ios_base::ate); 25 | if (istrm.is_open()) { 26 | RCLCPP_DEBUG(get_logger(), "Storing bookmark to file %s", path.c_str()); 27 | filePosition = istrm.tellg(); 28 | filePath = path; 29 | valid = true; 30 | istrm.close(); 31 | } 32 | else { 33 | throw std::runtime_error("Failed to open file " + path.string()); 34 | } 35 | } -------------------------------------------------------------------------------- /atos/modules/JournalControl/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "util.h" 7 | #include "rclcpp/rclcpp.hpp" 8 | #include "journalcontrol.hpp" 9 | 10 | int main(int argc, char **argv) { 11 | rclcpp::init(argc,argv); 12 | auto node = std::make_shared(); 13 | rclcpp::spin(node); 14 | return 0; 15 | } -------------------------------------------------------------------------------- /atos/modules/MQTTBridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.10) 6 | set(CMAKE_C_STANDARD 11) 7 | set(CMAKE_C_STANDARD_REQUIRED ON) 8 | set(CMAKE_CXX_STANDARD 17) 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 10 | 11 | project(mqtt_bridge) 12 | find_package(rclcpp REQUIRED) 13 | find_package(atos_interfaces REQUIRED) 14 | find_package(nlohmann_json 3.2.0 REQUIRED) 15 | find_package(PahoMqttCpp REQUIRED) 16 | find_package(fmt REQUIRED) 17 | 18 | # Define target names 19 | set(MQTT_BRIDGE_TARGET ${PROJECT_NAME}) 20 | set(ATOS_COMMON_LIBRARY ATOSCommon) 21 | get_target_property(COMMON_HEADERS ${ATOS_COMMON_LIBRARY} INCLUDE_DIRECTORIES) 22 | 23 | include(GNUInstallDirs) 24 | 25 | # Create project main executable target 26 | add_executable(${MQTT_BRIDGE_TARGET} 27 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 28 | ${CMAKE_CURRENT_SOURCE_DIR}/src/mqttbridge.cpp 29 | ) 30 | 31 | # Link project executable to util libraries 32 | target_link_libraries(${MQTT_BRIDGE_TARGET} 33 | ${ATOS_COMMON_LIBRARY} 34 | ${nlohmann_json} 35 | paho-mqttpp3 36 | paho-mqtt3as 37 | fmt 38 | ) 39 | 40 | target_include_directories(${MQTT_BRIDGE_TARGET} PUBLIC SYSTEM 41 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 42 | ${CMAKE_INSTALL_INCLUDEDIR} 43 | ${COMMON_HEADERS} 44 | ) 45 | 46 | # ROS specific rules 47 | ament_target_dependencies(${MQTT_BRIDGE_TARGET} 48 | rclcpp 49 | atos_interfaces 50 | ) 51 | 52 | # Installation rules 53 | install(CODE "MESSAGE(STATUS \"Installing target ${MQTT_BRIDGE_TARGET}\")") 54 | install(TARGETS ${MQTT_BRIDGE_TARGET} 55 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 56 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 57 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 58 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 59 | ) -------------------------------------------------------------------------------- /atos/modules/MQTTBridge/inc/Imqtt2ros.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | /** 5 | * @brief Struct containing variables related to a MQTT2ROS connection. 6 | * @details This struct is intended to be used in a map structure where the key 7 | * is the MQTT topic name. 8 | */ 9 | struct Mqtt2RosInterface { 10 | struct { 11 | int qos = 0; ///< MQTT QoS value 12 | } mqtt; ///< MQTT-related variables 13 | struct { 14 | std::string topic; ///< ROS topic 15 | std::string msg_type; ///< message type of publisher 16 | rclcpp::Publisher::SharedPtr 17 | publisher; ///< ROS publisher 18 | int queue_size = 1; ///< ROS publisher queue size 19 | bool is_stale = 20 | false; ///< whether a new generic publisher/subscriber is required 21 | } ros; ///< ROS-related variables 22 | }; -------------------------------------------------------------------------------- /atos/modules/MQTTBridge/inc/Iros2mqtt.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | /** 5 | * @brief Struct containing variables related to a MQTT2ROS connection. 6 | */ 7 | struct Ros2MqttInterface { 8 | struct { 9 | std::string topic; ///< MQTT topic 10 | int qos = 0; ///< MQTT QoS value 11 | } mqtt; ///< MQTT-related variables 12 | struct { 13 | std::string topic; ///< ROS topic 14 | std::string msg_type; ///< message type of subscriber 15 | rclcpp::GenericSubscription::SharedPtr subscriber; ///< ROS subscriber 16 | int queue_size = 1; ///< ROS subscriber queue size 17 | bool is_stale = 18 | false; ///< whether a new generic publisher/subscriber is required 19 | } ros; ///< ROS-related variables 20 | }; -------------------------------------------------------------------------------- /atos/modules/MQTTBridge/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #include "mqttbridge.hpp" 8 | 9 | int main(int argc, char** argv) { 10 | rclcpp::init(argc,argv); 11 | 12 | auto mb = std::make_shared(); 13 | if(rclcpp::ok()) { 14 | rclcpp::spin(mb); 15 | } 16 | rclcpp::shutdown(); 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /atos/modules/MonrRelay/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | set(CMAKE_CXX_STANDARD 17) 3 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 4 | set(CMAKE_C_STANDARD 11) 5 | set(CMAKE_C_STANDARD_REQUIRED ON) 6 | 7 | project(monr_relay) 8 | find_package(atos_interfaces REQUIRED) 9 | find_package(std_srvs REQUIRED) 10 | 11 | # Define target names 12 | set(MODULE_TARGET ${PROJECT_NAME}) 13 | 14 | set(COREUTILS_LIBRARY ATOSCoreUtil) 15 | set(COMMON_LIBRARY ATOSCommon) # Common library for ATOS with e.g. Trajectory class 16 | set(SOCKET_LIBRARY TCPUDPSocket) # Socket library for TCP/UDP communication 17 | 18 | get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES) 19 | get_target_property(COREUTILS_HEADERS ${COREUTILS_LIBRARY} INCLUDE_DIRECTORIES) 20 | 21 | include(GNUInstallDirs) 22 | 23 | # Create project main executable target 24 | add_executable(${MODULE_TARGET} 25 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 26 | ${CMAKE_CURRENT_SOURCE_DIR}/src/monr_relay.cpp 27 | ) 28 | # Link project executable to common libraries 29 | set(TARGET_LIBRARIES 30 | ${COREUTILS_LIBRARY} 31 | ${SOCKET_LIBRARY} 32 | ${COMMON_LIBRARY} 33 | ) 34 | target_link_libraries(${MODULE_TARGET} ${TARGET_LIBRARIES}) 35 | 36 | # Add include directories 37 | target_include_directories(${MODULE_TARGET} PUBLIC SYSTEM 38 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 39 | ${COMMON_HEADERS} 40 | ${COREUTILS_HEADERS} 41 | ) 42 | 43 | # ROS specific rules 44 | ament_target_dependencies(${MODULE_TARGET} 45 | rclcpp 46 | std_msgs 47 | std_srvs 48 | atos_interfaces 49 | ) 50 | 51 | # Installation rules 52 | install(CODE "MESSAGE(STATUS \"Installing target ${MODULE_TARGET}\")") 53 | install(TARGETS ${MODULE_TARGET} 54 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 55 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 56 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 57 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 58 | ) 59 | -------------------------------------------------------------------------------- /atos/modules/MonrRelay/inc/monr_relay.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "module.hpp" 14 | #include "roschannels/monitorchannel.hpp" 15 | #include "server.hpp" 16 | 17 | namespace monr_relay { 18 | 19 | /*! 20 | * \brief MonrRelay 21 | */ 22 | class MonrRelayModule : public Module { 23 | public: 24 | static inline std::string const moduleName = "monr_relay"; 25 | MonrRelayModule(); 26 | 27 | private: 28 | void relayMonitorMessage(ROSChannels::Monitor::message_type::SharedPtr message); 29 | ROSChannels::Monitor::SubAll monrSub; 30 | UDPServer udp_server; 31 | BasicSocket::HostInfo remote_host{}; 32 | }; 33 | 34 | } // namespace monr_relay 35 | -------------------------------------------------------------------------------- /atos/modules/MonrRelay/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "monr_relay.hpp" 2 | 3 | #include "rclcpp/rclcpp.hpp" 4 | 5 | int main(int argc, char** argv) { 6 | rclcpp::init(argc, argv); 7 | auto sm = std::make_shared(); 8 | rclcpp::spin(sm); 9 | rclcpp::shutdown(); 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /atos/modules/MonrRelay/src/monr_relay.cpp: -------------------------------------------------------------------------------- 1 | #include "monr_relay.hpp" 2 | 3 | #include "rclcpp/wait_for_message.hpp" 4 | #include "roschannels/commandchannels.hpp" 5 | 6 | namespace monr_relay { 7 | 8 | MonrRelayModule::MonrRelayModule() : 9 | Module(moduleName), 10 | monrSub(*this, [this](auto&& param) { MonrRelayModule::relayMonitorMessage(std::forward(param)); }) { 11 | BasicSocket::Address client_host{}; 12 | declare_parameter("client_host", "127.0.0.1"); 13 | get_parameter("client_host", client_host); 14 | 15 | BasicSocket::Port client_port{}; 16 | declare_parameter("client_port", 51234); 17 | get_parameter("client_port", client_port); 18 | 19 | remote_host = {client_host, client_port}; 20 | 21 | std::string server_host{}; 22 | declare_parameter("server_host", "127.0.0.1"); 23 | get_parameter("server_host", server_host); 24 | 25 | std::uint16_t server_port{}; 26 | declare_parameter("server_port", 51122); 27 | get_parameter("server_port", server_port); 28 | 29 | udp_server = UDPServer(server_host, server_port); 30 | } 31 | 32 | void MonrRelayModule::relayMonitorMessage(ROSChannels::Monitor::message_type::SharedPtr message) { 33 | std::vector out; 34 | out.reserve(message->raw_data.size()); 35 | std::copy(message->raw_data.begin(), message->raw_data.end(), std::back_inserter(out)); 36 | udp_server.sendto({out, remote_host}); 37 | } 38 | 39 | } // namespace monr_relay 40 | -------------------------------------------------------------------------------- /atos/modules/OSIAdapter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_C_STANDARD 11) 9 | set(CMAKE_C_STANDARD_REQUIRED ON) 10 | 11 | project(osi_adapter) 12 | 13 | find_package(rclcpp REQUIRED) 14 | find_package(std_msgs REQUIRED) 15 | find_package(atos_interfaces REQUIRED) 16 | 17 | # Define target names 18 | set(OSI_ADAPTER_TARGET ${PROJECT_NAME}) 19 | 20 | set(OSI_LIBRARY OSIHandler) 21 | set(ATOS_COMMON_LIBRARY ATOSCommon) 22 | set(SOCKET_LIBRARY TCPUDPSocket) 23 | 24 | get_target_property(OSI_HEADERS ${OSI_LIBRARY} INCLUDE_DIRECTORIES) 25 | get_target_property(SOCKET_HEADERS ${SOCKET_LIBRARY} INCLUDE_DIRECTORIES) 26 | 27 | include(GNUInstallDirs) 28 | 29 | # Create project main executable target 30 | add_executable(${OSI_ADAPTER_TARGET} 31 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 32 | ${CMAKE_CURRENT_SOURCE_DIR}/src/osiadapter.cpp 33 | ${CMAKE_CURRENT_SOURCE_DIR}/src/serverfactory.cpp 34 | ) 35 | # Link project executable to util libraries 36 | target_link_libraries(${OSI_ADAPTER_TARGET} 37 | ${ATOS_COMMON_LIBRARY} 38 | ${OSI_LIBRARY} 39 | ${SOCKET_LIBRARY} 40 | ) 41 | 42 | # ROS specific settings 43 | ament_target_dependencies(${OSI_ADAPTER_TARGET} 44 | rclcpp 45 | std_msgs 46 | atos_interfaces 47 | ) 48 | 49 | target_include_directories(${OSI_ADAPTER_TARGET} PUBLIC SYSTEM 50 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 51 | ${OSI_HEADERS} 52 | ${SOCKET_HEADERS} 53 | ) 54 | 55 | # Installation rules 56 | install(CODE "MESSAGE(STATUS \"Installing target ${OSI_ADAPTER_TARGET}\")") 57 | install(TARGETS ${OSI_ADAPTER_TARGET} 58 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 59 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 60 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 61 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 62 | ) -------------------------------------------------------------------------------- /atos/modules/OSIAdapter/inc/osiadapter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "module.hpp" 9 | #include "roschannels/commandchannels.hpp" 10 | #include "roschannels/monitorchannel.hpp" 11 | #include "osi_handler.hpp" 12 | #include "unordered_map" 13 | #include "serverfactory.hpp" 14 | #include 15 | 16 | class OSIAdapter : public Module 17 | { 18 | public: 19 | void initializeServer(); 20 | OSIAdapter(); 21 | ~OSIAdapter(); 22 | 23 | 24 | private: 25 | using TimeUnit = std::chrono::milliseconds; 26 | std::string address; 27 | uint16_t port; 28 | std::string protocol; 29 | uint16_t frequency; 30 | static inline std::string const moduleName = "osi_adapter"; 31 | 32 | void getParameters(); 33 | void sendOSIData(); 34 | std::vector makeOSIMessage(const std::vector& osiData); 35 | const OsiHandler::GlobalObjectGroundTruth_t makeOSIData(ROSChannels::Monitor::message_type& monr); 36 | 37 | std::unique_ptr server; 38 | rclcpp::TimerBase::SharedPtr timer; 39 | ROSChannels::ConnectedObjectIds::Sub connectedObjectIdsSub; //!< Publisher to report connected objects 40 | 41 | std::unordered_map lastMonitors; 42 | std::unordered_map lastMonitorTimes; 43 | std::unordered_map> monrSubscribers; 44 | 45 | void onConnectedObjectIdsMessage(const ROSChannels::ConnectedObjectIds::message_type::SharedPtr msg); 46 | void onMonitorMessage(const ROSChannels::Monitor::message_type::SharedPtr msg, uint32_t id); 47 | 48 | inline double linPosPrediction(const double position, const double velocity, const TimeUnit dt); 49 | void extrapolateMONR(ROSChannels::Monitor::message_type& monr, const TimeUnit dt); 50 | }; 51 | -------------------------------------------------------------------------------- /atos/modules/OSIAdapter/inc/serverfactory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include 9 | #include "server.hpp" 10 | 11 | 12 | class ServerFactory { 13 | 14 | public: 15 | ServerFactory(const std::string& address, const uint16_t& port, const std::string protocol); 16 | ~ServerFactory(); 17 | 18 | void createServer(); 19 | void setupServer(); 20 | void destroyServer(); 21 | void resetServer(); 22 | void sendData(const std::vector& data); 23 | 24 | 25 | private: 26 | std::string address; 27 | uint16_t port; 28 | std::string protocol; 29 | std::unique_ptr tcpServer; 30 | std::unique_ptr udpServer; 31 | std::shared_ptr socket; 32 | BasicSocket::HostInfo hostInfo; 33 | }; 34 | -------------------------------------------------------------------------------- /atos/modules/OSIAdapter/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include 7 | #include "osiadapter.hpp" 8 | 9 | 10 | int main(int argc, char** argv) { 11 | 12 | rclcpp::init(argc, argv); 13 | auto OSIAdapterNode = std::make_shared(); 14 | rclcpp::spin(OSIAdapterNode); 15 | rclcpp::shutdown(); 16 | 17 | return 0; 18 | } -------------------------------------------------------------------------------- /atos/modules/ObjectControl/inc/channel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "iso22133.h" 9 | #include "loggable.hpp" 10 | #include "roschannels/controlsignalchannel.hpp" 11 | #include "trajectory.hpp" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | struct MonitorMessage { 18 | uint32_t id; 19 | ObjectMonitorType object_monitor; 20 | std::vector raw_data{}; 21 | }; 22 | 23 | class Channel : public Loggable { 24 | public: 25 | Channel(const size_t bufferLength, const int type, rclcpp::Logger log, int id = 0, int transmitter = 0) : 26 | Loggable(log), 27 | channelType(type), 28 | objectId(id), 29 | transmitterId(transmitter), 30 | transmitBuffer(bufferLength, 0), 31 | receiveBuffer(bufferLength, 0) {} 32 | Channel(int type, rclcpp::Logger log, int id = 0) : 33 | Channel(1024, type, log, id) {} 34 | struct sockaddr_in addr = {}; 35 | int socket = -1; 36 | int channelType = 0; //!< SOCK_STREAM or SOCK_DGRAM 37 | int objectId = 0; 38 | int transmitterId = 0; 39 | int sentMessageCounter = 0; 40 | int receivedMessageCounter = 0; 41 | std::vector transmitBuffer; 42 | std::vector receiveBuffer; 43 | 44 | ISOMessageID pendingMessageType(bool awaitNext = false); 45 | std::string remoteIP() const; 46 | bool isValid() const { 47 | return socket != -1; 48 | } 49 | void connect(std::shared_future stopRequest, const std::chrono::milliseconds retryPeriod); 50 | void disconnect(); 51 | 52 | friend Channel& operator<<(Channel&, const HeabMessageDataType&); 53 | friend Channel& operator<<(Channel&, const ObjectSettingsType&); 54 | friend Channel& operator<<(Channel&, const ATOS::Trajectory&); 55 | friend Channel& operator<<(Channel&, const ObjectCommandType&); 56 | friend Channel& operator<<(Channel&, const StartMessageType&); 57 | friend Channel& operator<<(Channel&, const std::vector&); 58 | friend Channel& operator<<(Channel&, const ROSChannels::ControlSignal::message_type::SharedPtr csp); 59 | 60 | friend Channel& operator>>(Channel&, MonitorMessage&); 61 | friend Channel& operator>>(Channel&, ObjectPropertiesType&); 62 | friend Channel& operator>>(Channel&, GeneralResponseMessageType&); 63 | 64 | protected: 65 | MessageHeaderType* populateHeaderType(MessageHeaderType* header); 66 | }; 67 | -------------------------------------------------------------------------------- /atos/modules/ObjectControl/inc/objectconnection.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include "iso22133.h" 12 | #include "loggable.hpp" 13 | #include "channel.hpp" 14 | 15 | /*! 16 | * \brief The ObjectConnection class holds network connection data for 17 | * a single object, i.e. the two channels for command and 18 | * safety data. 19 | */ 20 | class ObjectConnection : public Loggable { 21 | public: 22 | Channel cmd; 23 | Channel mntr; 24 | 25 | ObjectConnection(rclcpp::Logger log, int id) 26 | : Loggable(log), 27 | cmd(SOCK_STREAM, log, id), 28 | mntr(SOCK_DGRAM, log, id) { 29 | pipe(interruptionPipeFds); 30 | } 31 | 32 | bool isValid() const; 33 | bool isConnected() const; 34 | void connect(std::shared_future stopRequest, 35 | const std::chrono::milliseconds retryPeriod); 36 | void disconnect(); 37 | ISOMessageID pendingMessageType(bool awaitNext = false); 38 | void interruptSocket() { 39 | int i = 1; 40 | write(interruptionPipeFds[1], &i, sizeof(i)); 41 | close(interruptionPipeFds[1]); 42 | } 43 | private: 44 | int interruptionPipeFds[2]; 45 | }; -------------------------------------------------------------------------------- /atos/modules/ObjectControl/inc/objectlistener.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "objectcontrol.hpp" 9 | #include "testobject.hpp" 10 | #include "loggable.hpp" 11 | #include 12 | 13 | class ObjectControl; 14 | class ObjectControlState; 15 | 16 | class ObjectListener : public Loggable 17 | { 18 | public: 19 | ObjectListener( 20 | ObjectControl*, 21 | std::shared_ptr, 22 | rclcpp::Logger 23 | ); 24 | ~ObjectListener(); 25 | private: 26 | std::shared_ptr obj; 27 | ObjectControl* handler; 28 | std::thread listener; 29 | 30 | void listen(); 31 | bool quit = false; 32 | }; 33 | 34 | -------------------------------------------------------------------------------- /atos/modules/ObjectControl/inc/relativeanchor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "relativetestobject.hpp" 9 | #include "roschannels/monitorchannel.hpp" 10 | 11 | class RelativeAnchor : public TestObject { 12 | public: 13 | RelativeAnchor(uint32_t id); 14 | RelativeAnchor(const RelativeAnchor&) = delete; 15 | RelativeAnchor(RelativeAnchor&&); 16 | 17 | RelativeAnchor& operator=(const RelativeAnchor&) = delete; 18 | RelativeAnchor& operator=(RelativeAnchor&&) = default; 19 | 20 | virtual void publishMonr(const ROSChannels::Monitor::message_type) override; 21 | 22 | private: 23 | ROSChannels::Monitor::AnchorPub anchorPub; 24 | }; 25 | -------------------------------------------------------------------------------- /atos/modules/ObjectControl/inc/relativetestobject.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "testobject.hpp" 9 | #include "roschannels/monitorchannel.hpp" 10 | #include "positioning.h" 11 | 12 | class RelativeTestObject : public TestObject { 13 | public: 14 | RelativeTestObject(uint32_t id); 15 | RelativeTestObject(const RelativeTestObject&) = delete; 16 | RelativeTestObject(RelativeTestObject&&); 17 | 18 | RelativeTestObject& operator=(const RelativeTestObject&) = delete; 19 | RelativeTestObject& operator=(RelativeTestObject&&) = default; 20 | 21 | private: 22 | virtual ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, 23 | const ObjectMonitorType& anchor, 24 | const bool debug); 25 | virtual MonitorMessage readMonitorMessage() override; 26 | 27 | ROSChannels::Monitor::AnchorSub anchorSub; 28 | ObjectMonitorType lastAnchorMonr; 29 | void updateAnchor(const ROSChannels::Monitor::message_type::SharedPtr); 30 | }; 31 | -------------------------------------------------------------------------------- /atos/modules/ObjectControl/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "objectcontrol.hpp" 7 | #include 8 | 9 | int main(int argc, char **argv) { 10 | rclcpp::init(argc,argv); 11 | auto exec = std::make_shared(); 12 | auto obc = std::make_shared(exec); 13 | exec->add_node(obc); 14 | exec->spin(); 15 | rclcpp::shutdown(); 16 | 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /atos/modules/ObjectControl/src/objectlistener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "objectlistener.hpp" 7 | #include "objectcontrol.hpp" 8 | 9 | ObjectListener::ObjectListener(ObjectControl* sh, std::shared_ptr ob, rclcpp::Logger log) : 10 | Loggable(log), 11 | obj(ob), 12 | handler(sh) { 13 | 14 | if (!obj->isConnected()) { 15 | throw std::invalid_argument("Attempted to start listener for disconnected object"); 16 | } 17 | RCLCPP_DEBUG(get_logger(), "Starting listener thread for object %u", ob->getTransmitterID()); 18 | listener = std::thread(&ObjectListener::listen, this); 19 | } 20 | 21 | ObjectListener::~ObjectListener() { 22 | this->quit = true; 23 | RCLCPP_DEBUG(get_logger(), "Awaiting thread exit"); 24 | 25 | // Interrupt socket to unblock readMonitorMessage, and allow thread to exit gracefully 26 | obj->interruptSocket(); 27 | listener.join(); 28 | RCLCPP_DEBUG(get_logger(), "Thread exited"); 29 | } 30 | 31 | void ObjectListener::listen() { 32 | try { 33 | while (!this->quit) { 34 | //handle incoming iso22133 messages 35 | obj->handleISOMessage(true); 36 | } 37 | } catch (std::invalid_argument& e) { 38 | RCLCPP_ERROR(get_logger(), e.what()); // TODO: add comment explaining this case.. 39 | } catch (std::range_error& e){ 40 | RCLCPP_DEBUG(get_logger(), e.what()); // Socket was interrupted intentionally, exit gracefully 41 | } catch (std::runtime_error& e) { 42 | RCLCPP_ERROR(get_logger(), e.what()); 43 | obj->disconnect(); 44 | handler->sm.process_event(state_machine::events::DisconnectedFromObject{obj->getTransmitterID()}); 45 | } 46 | RCLCPP_INFO(get_logger(), "Listener thread for object %u exiting", obj->getTransmitterID()); 47 | } 48 | -------------------------------------------------------------------------------- /atos/modules/ObjectControl/src/relativeanchor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #include "testobject.hpp" 8 | #include 9 | 10 | RelativeAnchor::RelativeAnchor(uint32_t id) : 11 | TestObject(id), 12 | anchorPub(*this) {} 13 | 14 | RelativeAnchor::RelativeAnchor(RelativeAnchor&& other) : 15 | TestObject(std::move(other)), 16 | anchorPub(std::move(other.anchorPub)) {} 17 | 18 | RelativeAnchor::publishMonr(const ROSChannels::Monitor::message_type monr) { 19 | monrPub.publish(monr); 20 | monrPubAll.publish(monr); 21 | anchorPub.publish(monr); 22 | } 23 | -------------------------------------------------------------------------------- /atos/modules/OpenScenarioGateway/custom_command_action.py: -------------------------------------------------------------------------------- 1 | class CustomCommandAction: 2 | def __init__(self, type: str, content: str): 3 | self.type = type 4 | self.content = content 5 | 6 | def __eq__(self, other): 7 | return self.type == other.type and self.content == other.content 8 | -------------------------------------------------------------------------------- /atos/modules/PointcloudPublisher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_C_STANDARD 11) 9 | set(CMAKE_C_STANDARD_REQUIRED ON) 10 | 11 | project(pointcloud_publisher) 12 | 13 | find_package(rclcpp REQUIRED) 14 | find_package(std_msgs REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(atos_interfaces REQUIRED) 17 | find_package(PCL 1.10 REQUIRED) 18 | find_package(pcl_conversions REQUIRED) 19 | 20 | # Define target names 21 | set(POINTCLOUD_PUBLISHER_TARGET ${PROJECT_NAME}) 22 | 23 | set(ATOS_COMMON_LIBRARY ATOSCommon) 24 | get_target_property(COMMON_HEADERS ${ATOS_COMMON_LIBRARY} INCLUDE_DIRECTORIES) 25 | 26 | include(GNUInstallDirs) 27 | 28 | include_directories(${PCL_INCLUDE_DIRS}) 29 | link_directories(${PCL_LIBRARY_DIRS}) 30 | add_definitions(${PCL_DEFINITIONS}) 31 | 32 | # Create project main executable target 33 | add_executable(${POINTCLOUD_PUBLISHER_TARGET} 34 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 35 | ${CMAKE_CURRENT_SOURCE_DIR}/src/pointcloudpublisher.cpp 36 | ) 37 | # Link project executable to util libraries 38 | target_link_libraries(${POINTCLOUD_PUBLISHER_TARGET} 39 | ${ATOS_COMMON_LIBRARY} 40 | ${ATOS_COMMON_LIBRARY} 41 | ${PCL_LIBRARIES} 42 | ) 43 | 44 | # ROS specific settings 45 | ament_target_dependencies(${POINTCLOUD_PUBLISHER_TARGET} 46 | rclcpp 47 | std_msgs 48 | sensor_msgs 49 | atos_interfaces 50 | pcl_conversions 51 | ) 52 | 53 | target_include_directories(${POINTCLOUD_PUBLISHER_TARGET} PUBLIC SYSTEM 54 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 55 | ${COMMON_HEADERS} 56 | ) 57 | 58 | # Installation rules 59 | install(CODE "MESSAGE(STATUS \"Installing target ${OSI_ADAPTER_TARGET}\")") 60 | install(TARGETS ${POINTCLOUD_PUBLISHER_TARGET} 61 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 62 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 63 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 64 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 65 | ) -------------------------------------------------------------------------------- /atos/modules/PointcloudPublisher/inc/pointcloudpublisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include "module.hpp" 9 | #include "roschannels/pointcloudchannel.hpp" 10 | #include "roschannels/commandchannels.hpp" 11 | #include 12 | #include 13 | 14 | class PointcloudPublisher : public Module { 15 | 16 | public: 17 | PointcloudPublisher(); 18 | ~PointcloudPublisher(); 19 | 20 | private: 21 | static inline std::string const moduleName = "pointcloud_publisher"; 22 | ROSChannels::Init::Sub initSub; 23 | std::map> pointcloudPubs; 24 | 25 | std::vector pointcloudFiles; 26 | std::map>> pointclouds; 27 | 28 | void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; 29 | void initialize(); 30 | void readPointcloudParams(); 31 | void loadPointClouds(); 32 | void createPublishers(); 33 | std::string getPublisherTopicName(const std::string &path) const; 34 | }; -------------------------------------------------------------------------------- /atos/modules/PointcloudPublisher/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include 7 | #include "pointcloudpublisher.hpp" 8 | 9 | int main(int argc, char **argv){ 10 | 11 | rclcpp::init(argc, argv); 12 | auto pointcloudPublisherNode = std::make_shared(); 13 | rclcpp::spin(pointcloudPublisherNode); 14 | rclcpp::shutdown(); 15 | 16 | return 0; 17 | } -------------------------------------------------------------------------------- /atos/modules/RESTBridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | set(CMAKE_CXX_STANDARD 17) 3 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 4 | set(CMAKE_C_STANDARD 11) 5 | set(CMAKE_C_STANDARD_REQUIRED ON) 6 | 7 | project(rest_bridge) 8 | find_package(atos_interfaces REQUIRED) 9 | find_package(std_srvs REQUIRED) 10 | 11 | # Define target names 12 | set(REST_BRIDGE_TARGET ${PROJECT_NAME}) 13 | 14 | set(COREUTILS_LIBRARY ATOSCoreUtil) 15 | set(COMMON_LIBRARY ATOSCommon) # Common library for ATOS with e.g. Trajectory class 16 | set(SOCKET_LIBRARY TCPUDPSocket) # Socket library for TCP/UDP communication 17 | 18 | get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES) 19 | get_target_property(COREUTILS_HEADERS ${COREUTILS_LIBRARY} INCLUDE_DIRECTORIES) 20 | 21 | include(GNUInstallDirs) 22 | 23 | # Create project main executable target 24 | add_executable(${REST_BRIDGE_TARGET} 25 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 26 | ${CMAKE_CURRENT_SOURCE_DIR}/src/restbridge.cpp 27 | ) 28 | # Link project executable to common libraries 29 | set(TARGET_LIBRARIES 30 | ${COREUTILS_LIBRARY} 31 | ${SOCKET_LIBRARY} 32 | ${COMMON_LIBRARY} 33 | ${CURL_LIBRARIES} curl) 34 | 35 | target_link_libraries(${REST_BRIDGE_TARGET} ${TARGET_LIBRARIES}) 36 | 37 | # Add include directories 38 | target_include_directories(${REST_BRIDGE_TARGET} PUBLIC SYSTEM 39 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 40 | ${COMMON_HEADERS} 41 | ${COREUTILS_HEADERS} 42 | ) 43 | 44 | # ROS specific rules 45 | ament_target_dependencies(${REST_BRIDGE_TARGET} 46 | rclcpp 47 | std_msgs 48 | std_srvs 49 | atos_interfaces 50 | ) 51 | 52 | # Installation rules 53 | install(CODE "MESSAGE(STATUS \"Installing target ${REST_BRIDGE_TARGET}\")") 54 | install(TARGETS ${REST_BRIDGE_TARGET} 55 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 56 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 57 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 58 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 59 | ) 60 | -------------------------------------------------------------------------------- /atos/modules/RESTBridge/inc/restbridge.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "module.hpp" 10 | #include "roschannels/customcommandaction.hpp" 11 | #include 12 | #include 13 | 14 | using json = nlohmann::json; 15 | 16 | /*! 17 | * \brief The RESTBridge is a ros2 node that demonstrates how to use the Module 18 | * class 19 | */ 20 | class RESTBridge : public Module { 21 | public: 22 | static inline std::string const moduleName = "rest_bridge"; 23 | RESTBridge(); 24 | ~RESTBridge(); 25 | 26 | protected: 27 | void onCustomCommandAction( 28 | const atos_interfaces::msg::CustomCommandAction::SharedPtr msg); 29 | 30 | private: 31 | ROSChannels::CustomCommandAction::Sub 32 | customCommandActionMsgSub; //!< Subscriber to icdc messages requests 33 | 34 | json parseJsonData(std::string &msg); 35 | void POST(const std::string &endpoint, const json &data); 36 | 37 | CURL *curl_handle; 38 | }; 39 | -------------------------------------------------------------------------------- /atos/modules/RESTBridge/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "restbridge.hpp" 3 | 4 | int main(int argc, char **argv) { 5 | rclcpp::init(argc, argv); 6 | auto rb = std::make_shared(); 7 | rclcpp::spin(rb); 8 | rclcpp::shutdown(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /atos/modules/RESTBridge/src/restbridge.cpp: -------------------------------------------------------------------------------- 1 | #include "restbridge.hpp" 2 | 3 | using namespace ROSChannels; 4 | using namespace std::chrono_literals; 5 | using namespace std::placeholders; 6 | 7 | RESTBridge::RESTBridge() 8 | : Module(moduleName), 9 | customCommandActionMsgSub( 10 | *this, std::bind(&RESTBridge::onCustomCommandAction, this, _1)) { 11 | curl_global_init(CURL_GLOBAL_ALL); 12 | curl_handle = curl_easy_init(); 13 | } 14 | 15 | RESTBridge::~RESTBridge() { 16 | curl_easy_cleanup(curl_handle); 17 | curl_global_cleanup(); 18 | } 19 | 20 | void RESTBridge::onCustomCommandAction( 21 | const atos_interfaces::msg::CustomCommandAction::SharedPtr msg) { 22 | if (msg->type == atos_interfaces::msg::CustomCommandAction::POST_JSON) { 23 | RCLCPP_INFO(get_logger(), "Received POST_JSON command: %s", 24 | msg->content.c_str()); 25 | json jsonData = parseJsonData(msg->content); 26 | POST(jsonData["endpoint"].get(), jsonData["data"]); 27 | } 28 | } 29 | 30 | json RESTBridge::parseJsonData(std::string &msg) { 31 | // Parse the message and return the REST API message 32 | std::replace(msg.begin(), msg.end(), '\'', 33 | '\"'); // Replace single quotes with double quotes to be able to 34 | // parse the message 35 | json j = json::parse(msg); 36 | return j; 37 | } 38 | 39 | void RESTBridge::POST(const std::string &endpoint, const json &data) { 40 | // Send A POST request to the specified endpoint with the specified data, Use 41 | // hardcoded headers for now 42 | CURLcode res; 43 | 44 | if (curl_handle) { 45 | std::string json_str = data.dump(); // Store the JSON string 46 | const char *json_data = json_str.c_str(); // Get the C-string pointer 47 | curl_easy_setopt(curl_handle, CURLOPT_URL, endpoint.c_str()); 48 | curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, json_data); 49 | 50 | struct curl_slist *headers = NULL; 51 | headers = curl_slist_append(headers, "Accept: application/json"); 52 | headers = curl_slist_append(headers, "Content-Type: application/json"); 53 | headers = curl_slist_append(headers, "charset: utf-8"); 54 | curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, headers); 55 | 56 | // Perform the request, res will get the return code 57 | res = curl_easy_perform(curl_handle); 58 | 59 | // Check for errors 60 | if (res != CURLE_OK) { 61 | RCLCPP_ERROR(get_logger(), "curl_easy_perform() failed: %s\n", 62 | curl_easy_strerror(res)); 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /atos/modules/SampleModule/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | set(CMAKE_CXX_STANDARD 17) 3 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 4 | set(CMAKE_C_STANDARD 11) 5 | set(CMAKE_C_STANDARD_REQUIRED ON) 6 | 7 | project(sample_module) 8 | find_package(atos_interfaces REQUIRED) 9 | find_package(std_srvs REQUIRED) 10 | 11 | # Define target names 12 | set(SAMPLE_MODULE_TARGET ${PROJECT_NAME}) 13 | 14 | set(COREUTILS_LIBRARY ATOSCoreUtil) 15 | set(COMMON_LIBRARY ATOSCommon) # Common library for ATOS with e.g. Trajectory class 16 | set(SOCKET_LIBRARY TCPUDPSocket) # Socket library for TCP/UDP communication 17 | 18 | get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES) 19 | get_target_property(COREUTILS_HEADERS ${COREUTILS_LIBRARY} INCLUDE_DIRECTORIES) 20 | 21 | include(GNUInstallDirs) 22 | 23 | # Create project main executable target 24 | add_executable(${SAMPLE_MODULE_TARGET} 25 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 26 | ${CMAKE_CURRENT_SOURCE_DIR}/src/samplemodule.cpp 27 | ) 28 | # Link project executable to common libraries 29 | set(TARGET_LIBRARIES 30 | ${COREUTILS_LIBRARY} 31 | ${SOCKET_LIBRARY} 32 | ${COMMON_LIBRARY} 33 | ) 34 | target_link_libraries(${SAMPLE_MODULE_TARGET} ${TARGET_LIBRARIES}) 35 | 36 | # Add include directories 37 | target_include_directories(${SAMPLE_MODULE_TARGET} PUBLIC SYSTEM 38 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 39 | ${COMMON_HEADERS} 40 | ${COREUTILS_HEADERS} 41 | ) 42 | 43 | # ROS specific rules 44 | ament_target_dependencies(${SAMPLE_MODULE_TARGET} 45 | rclcpp 46 | std_msgs 47 | std_srvs 48 | atos_interfaces 49 | ) 50 | 51 | if(BUILD_TESTING) 52 | find_package(ament_cmake_ros REQUIRED) 53 | file(GLOB TESTFILES "test/*.cpp") 54 | set(SRCFILES "src/samplemodule.cpp") 55 | 56 | ament_add_ros_isolated_gtest(${SAMPLE_MODULE_TARGET}_test ${TESTFILES} ${SRCFILES}) 57 | target_link_libraries(${SAMPLE_MODULE_TARGET}_test ${TARGET_LIBRARIES}) 58 | target_include_directories(${SAMPLE_MODULE_TARGET}_test PUBLIC 59 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 60 | ${COMMON_HEADERS} 61 | ) 62 | 63 | # ROS specific rules 64 | ament_target_dependencies(${SAMPLE_MODULE_TARGET}_test 65 | rclcpp 66 | std_msgs 67 | std_srvs 68 | atos_interfaces 69 | ) 70 | endif() 71 | 72 | # Installation rules 73 | install(CODE "MESSAGE(STATUS \"Installing target ${SAMPLE_MODULE_TARGET}\")") 74 | install(TARGETS ${SAMPLE_MODULE_TARGET} 75 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 76 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 77 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 78 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 79 | ) 80 | -------------------------------------------------------------------------------- /atos/modules/SampleModule/inc/samplemodule.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include "roschannels/test_channels.hpp" 12 | #include "module.hpp" 13 | #include "server.hpp" 14 | 15 | /*! 16 | * \brief The SampleModule is a ros2 node that demonstrates how to use the Module class 17 | */ 18 | class SampleModule : public Module{ 19 | public: 20 | static inline std::string const moduleName = "sample_module"; 21 | SampleModule(); 22 | std::vector getObjectIds(); 23 | bool getAborting() const { return aborting_; } 24 | 25 | private: 26 | ROSChannels::Init::Sub initSub; 27 | ROSChannels::Abort::Sub abortSub; 28 | ROSChannels::AllClear::Sub allClearSub; 29 | ROSChannels::SampleModuleTestForInitResponse::Pub smOnInitResponsePub; 30 | rclcpp::Service::SharedPtr service_server; 31 | 32 | void onInitMessage(ROSChannels::Init::message_type::SharedPtr) override; 33 | void onAbortMessage(ROSChannels::Abort::message_type::SharedPtr) override; 34 | void onAllClearMessage(ROSChannels::AllClear::message_type::SharedPtr) override; 35 | void OnCallbackSetBool(const std::shared_ptr request, 36 | std::shared_ptr response); 37 | 38 | std::vector objectIds; 39 | bool aborting_ = false; 40 | }; 41 | -------------------------------------------------------------------------------- /atos/modules/SampleModule/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "samplemodule.hpp" 3 | 4 | 5 | int main(int argc, char** argv) { 6 | rclcpp::init(argc,argv); 7 | auto sm = std::make_shared(); 8 | rclcpp::spin(sm); 9 | rclcpp::shutdown(); 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /atos/modules/SampleModule/src/samplemodule.cpp: -------------------------------------------------------------------------------- 1 | #include "samplemodule.hpp" 2 | #include "roschannels/commandchannels.hpp" 3 | #include "rclcpp/wait_for_message.hpp" 4 | 5 | using namespace ROSChannels; 6 | using namespace std::chrono_literals; 7 | using namespace std::placeholders; 8 | 9 | SampleModule::SampleModule() : 10 | Module(moduleName), 11 | initSub(*this, std::bind(&SampleModule::onInitMessage, this, _1)), 12 | abortSub(*this, std::bind(&SampleModule::onAbortMessage, this, _1)), 13 | allClearSub(*this, std::bind(&SampleModule::onAllClearMessage, this, _1)), 14 | smOnInitResponsePub(*this) 15 | { 16 | service_server = create_service( 17 | "/sample_module_test_service", std::bind(&SampleModule::OnCallbackSetBool, this, _1, _2)); 18 | } 19 | 20 | //! Message queue callbacks 21 | void SampleModule::onInitMessage(const Init::message_type::SharedPtr) { 22 | // Callback executed when this module receives a test is initialized 23 | 24 | // Get the object id of the objects in the scenario 25 | ConnectedObjectIds::message_type connectedObjectIds; 26 | // Wait for a single message containing a vector of ids, at most 1000ms 27 | rclcpp::wait_for_message(connectedObjectIds, shared_from_this(), std::string(get_namespace()) + "connected_object_ids", 1000ms); 28 | // Populate a list of object IDs 29 | for (auto id : connectedObjectIds.ids) { 30 | objectIds.push_back(id); 31 | } 32 | smOnInitResponsePub.publish(std_msgs::msg::Empty()); 33 | } 34 | 35 | void SampleModule::onAbortMessage(const Abort::message_type::SharedPtr) { 36 | aborting_ = true; 37 | } 38 | 39 | void SampleModule::onAllClearMessage(const AllClear::message_type::SharedPtr) {} 40 | 41 | 42 | void SampleModule::OnCallbackSetBool(const std::shared_ptr, 43 | std::shared_ptr response) 44 | { 45 | response->message = "succeeded"; 46 | response->success = true; 47 | } 48 | 49 | /*! \brief returns the object ids of the objects in the scenario 50 | * \return a vector of object ids 51 | */ 52 | std::vector SampleModule::getObjectIds() { 53 | return objectIds; 54 | } -------------------------------------------------------------------------------- /atos/modules/SampleModule/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | int main(int argc, char** argv) 4 | { 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } -------------------------------------------------------------------------------- /atos/modules/SystemControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_C_STANDARD 11) 9 | set(CMAKE_C_STANDARD_REQUIRED ON) 10 | 11 | 12 | project(system_control) 13 | find_package(rclcpp REQUIRED) 14 | find_package(atos_interfaces REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | 17 | # Define target names 18 | set(SYSTEM_CONTROL_TARGET ${PROJECT_NAME}) 19 | 20 | set(COREUTILS_LIBRARY ATOSCoreUtil) 21 | set(TIME_LIBRARY ATOSTime) 22 | set(ATOS_COMMON_LIBRARY ATOSCommon) 23 | 24 | include(GNUInstallDirs) 25 | 26 | # Create project main executable target 27 | add_executable(${SYSTEM_CONTROL_TARGET} 28 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 29 | ${CMAKE_CURRENT_SOURCE_DIR}/src/systemcontrol.cpp 30 | ) 31 | 32 | ament_target_dependencies(${SYSTEM_CONTROL_TARGET} 33 | rclcpp 34 | std_msgs 35 | atos_interfaces 36 | ) 37 | 38 | target_link_libraries(${SYSTEM_CONTROL_TARGET} 39 | ${TIME_LIBRARY} 40 | ${COREUTILS_LIBRARY} 41 | ${ATOS_COMMON_LIBRARY} 42 | ) 43 | 44 | target_include_directories(${SYSTEM_CONTROL_TARGET} PUBLIC SYSTEM 45 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 46 | ${TIME_HEADERS} 47 | ) 48 | 49 | # Installation rules 50 | install(CODE "MESSAGE(STATUS \"Installing target ${SYSTEM_CONTROL_TARGET}\")") 51 | 52 | install(TARGETS ${SYSTEM_CONTROL_TARGET} 53 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 54 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 55 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 56 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 57 | ) 58 | 59 | -------------------------------------------------------------------------------- /atos/modules/SystemControl/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "systemcontrol.hpp" 7 | #include "rclcpp/executor.hpp" 8 | 9 | using namespace std::chrono; 10 | 11 | /* decl */ 12 | static std::shared_ptr sc; 13 | static void systemcontrol_task(int argc, char** argv); 14 | 15 | int main(int argc, char** argv){ 16 | systemcontrol_task(argc, argv); 17 | return 0; 18 | } 19 | 20 | /* 21 | Initializes Logs, Shared memory and mode, then runs the main loop 22 | */ 23 | void systemcontrol_task(int argc, char** argv){ 24 | // Initialize ROS node 25 | rclcpp::init(argc, argv); 26 | sc = std::make_shared(); 27 | sc->initialize(); 28 | 29 | // Set up signal handlers 30 | auto f = [](int signo) -> void {sc->signalHandler(signo);}; 31 | if (std::signal(SIGINT,f) == SIG_ERR){ 32 | util_error("Unable to initialize signal handler"); 33 | } 34 | 35 | rclcpp::executors::SingleThreadedExecutor executor; 36 | 37 | while (rclcpp::ok() && !sc->shouldExit()){ 38 | sc->receiveUserCommand(); 39 | sc->processUserCommand(); 40 | sc->sendUnsolicitedData(); 41 | sc->pollModuleStatus(); 42 | 43 | // If SystemControl is not in work state and clientResult < 0 then wait at most QUEUE_EMPTY_POLL_PERIOD for a msg 44 | // else keep running at full speed. 45 | const int64_t sleeptime=sc->isWorking() ? 0 : sc->getQueueEmptyPollPeriod(); 46 | executor.spin_node_once(sc,duration(sleeptime)); 47 | } 48 | ReadWriteAccess_t dataDictOperationResult = DataDictionaryDestructor(); 49 | if (dataDictOperationResult != WRITE_OK && dataDictOperationResult != READ_WRITE_OK) { 50 | util_error("Unable to clear shared memory space"); 51 | } 52 | RCLCPP_INFO(sc->get_logger(), "System control exiting"); 53 | rclcpp::shutdown(); 54 | } 55 | -------------------------------------------------------------------------------- /atos/modules/TrajectoryletStreamer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Source Code Form is subject to the terms of the Mozilla Public 2 | # License, v. 2.0. If a copy of the MPL was not distributed with this 3 | # file, You can obtain one at https://mozilla.org/MPL/2.0/. 4 | 5 | cmake_minimum_required(VERSION 3.1) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_C_STANDARD 11) 9 | set(CMAKE_C_STANDARD_REQUIRED ON) 10 | 11 | project(trajectorylet_streamer) 12 | 13 | find_package(rclcpp REQUIRED) 14 | find_package(atos_interfaces REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(nav_msgs REQUIRED) 17 | find_package(tf2_geometry_msgs REQUIRED) 18 | find_package(tf2 REQUIRED) 19 | 20 | set(ATOS_COMMON_LIBRARY ATOSCommon) 21 | get_target_property(COMMON_HEADERS ${ATOS_COMMON_LIBRARY} INCLUDE_DIRECTORIES) 22 | 23 | # Define target names 24 | set(TRAJECTORYLET_STREAMER_TARGET ${PROJECT_NAME}) 25 | 26 | include(GNUInstallDirs) 27 | 28 | # Create project main executable target 29 | add_executable(${TRAJECTORYLET_STREAMER_TARGET} 30 | ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp 31 | ${CMAKE_CURRENT_SOURCE_DIR}/src/trajectoryletstreamer.cpp 32 | ${CMAKE_CURRENT_SOURCE_DIR}/src/trajectorypublisher.cpp 33 | ) 34 | # Link project executable to util libraries 35 | target_link_libraries(${TRAJECTORYLET_STREAMER_TARGET} 36 | ${ATOS_COMMON_LIBRARY} 37 | ) 38 | target_include_directories(${TRAJECTORYLET_STREAMER_TARGET} PUBLIC SYSTEM 39 | ${CMAKE_CURRENT_SOURCE_DIR}/inc 40 | ${COMMON_HEADERS} 41 | ) 42 | 43 | # ROS specific settings 44 | ament_target_dependencies(${TRAJECTORYLET_STREAMER_TARGET} 45 | rclcpp 46 | std_msgs 47 | nav_msgs 48 | tf2_geometry_msgs 49 | atos_interfaces 50 | tf2 51 | ) 52 | 53 | # Installation rules 54 | install(CODE "MESSAGE(STATUS \"Installing target ${TRAJECTORYLET_STREAMER_TARGET}\")") 55 | install(TARGETS ${TRAJECTORYLET_STREAMER_TARGET} 56 | RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" 57 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 58 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 59 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 60 | ) 61 | 62 | -------------------------------------------------------------------------------- /atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "atos_interfaces/srv/get_object_ids.hpp" 11 | #include "atos_interfaces/srv/get_object_trajectory.hpp" 12 | #include "module.hpp" 13 | #include "objectconfig.hpp" 14 | #include "roschannels/commandchannels.hpp" 15 | #include "trajectory.hpp" 16 | #include "trajectorypublisher.hpp" 17 | 18 | namespace ATOS { 19 | 20 | class TrajectoryletStreamer : public Module { 21 | public: 22 | TrajectoryletStreamer(); 23 | 24 | private: 25 | static inline std::string const moduleName = "trajectorylet_streamer"; 26 | void onInitMessage(const ROSChannels::Init::message_type::SharedPtr); 27 | void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr); 28 | void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) override; 29 | void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) override; 30 | 31 | void loadObjectFiles(); 32 | void clearScenario(); 33 | 34 | ROSChannels::Init::Sub initSub; 35 | ROSChannels::ObjectsConnected::Sub connectedSub; 36 | ROSChannels::Abort::Sub abortSub; 37 | ROSChannels::Stop::Sub stopSub; 38 | 39 | rclcpp::Client::SharedPtr idClient; //!< Client to request object ids 40 | rclcpp::Client::SharedPtr 41 | trajectoryClient; //!< Client to request object trajectories 42 | 43 | std::vector> publishers; 44 | 45 | std::map> trajectories; 46 | 47 | std::chrono::milliseconds chunkLength; 48 | }; 49 | 50 | } // namespace ATOS 51 | -------------------------------------------------------------------------------- /atos/modules/TrajectoryletStreamer/inc/trajectorypublisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include "objectconfig.hpp" 7 | #include "roschannels/pathchannel.hpp" 8 | #include "roschannels/commandchannels.hpp" 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace ATOS { 16 | 17 | class TrajectoryPublisher { 18 | private: 19 | typedef std::pair::const_iterator, 20 | std::vector::const_iterator> Chunk; 21 | public: 22 | TrajectoryPublisher( 23 | rclcpp::Node& node, 24 | const Trajectory&, 25 | const uint32_t objectId, 26 | const std::chrono::milliseconds chunkLength 27 | = std::chrono::milliseconds(0)); 28 | 29 | void setChunkLength(std::chrono::milliseconds chunkLength) { 30 | this->chunkLength = chunkLength; 31 | } 32 | private: 33 | ROSChannels::Path::Pub pub; 34 | ROSChannels::StartObject::Sub startObjectSub; 35 | std::shared_ptr timer; 36 | 37 | std::chrono::milliseconds chunkLength = std::chrono::milliseconds(0); 38 | std::chrono::milliseconds publishPeriod = std::chrono::milliseconds(50); 39 | std::unique_ptr startTime; 40 | std::chrono::steady_clock::time_point lastPublishTime; 41 | 42 | std::unique_ptr traj; 43 | Chunk lastPublishedChunk; 44 | 45 | void publishChunk(); 46 | void getStartObjectMsg(const atos_interfaces::msg::ObjectTriggerStart::SharedPtr msg); 47 | nav_msgs::msg::Path chunkToPath(Chunk chunk, std::chrono::steady_clock::time_point); 48 | Chunk extractChunk(std::chrono::steady_clock::duration beginTime, std::chrono::steady_clock::duration endTime); 49 | 50 | }; 51 | 52 | } // namespace ATOS 53 | -------------------------------------------------------------------------------- /atos/modules/TrajectoryletStreamer/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #include 7 | #include "trajectoryletstreamer.hpp" 8 | 9 | int main(int argc, char **argv) { 10 | rclcpp::init(argc,argv); 11 | auto ts = std::make_shared(); 12 | rclcpp::spin(ts); 13 | rclcpp::shutdown(); 14 | 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /atos/modules/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/atos/modules/__init__.py -------------------------------------------------------------------------------- /atos/modules/conftest.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | import pytest 3 | 4 | 5 | @pytest.fixture() 6 | def get_file_path(): 7 | def _(file_path: str): 8 | """ 9 | Returns the absolute file path. 10 | 11 | Args: 12 | file_path (str): The relative file path. 13 | 14 | Returns: 15 | str: The absolute file path. 16 | """ 17 | return (Path(__file__).parent / file_path).absolute() 18 | 19 | return _ 20 | -------------------------------------------------------------------------------- /atos/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | atos 5 | 1.0.0 6 | The ATOS package 7 | MPL 2.0 8 | lukas 9 | 10 | Lukas Wikander 11 | Sebastian Loh Lindholm 12 | Viktor Johansson 13 | Adam Eriksson 14 | Alfred Aronsson 15 | Jesper Blidkvist 16 | Oscar Johansson 17 | Frida Reichenberg 18 | Victor Jarlow 19 | Samuel Thorén 20 | 21 | ament_cmake 22 | ament_cmake_python 23 | rclcpp 24 | rclpy 25 | std_msgs 26 | nav_msgs 27 | std_srvs 28 | atos_interfaces 29 | atos_gui 30 | rosbridge_suite 31 | sensor_msgs 32 | geometry_msgs 33 | foxglove_msgs 34 | foxglove_bridge 35 | pcl_conversions 36 | fmt 37 | doxygen 38 | 39 | ament_cmake_ros 40 | ament_cmake_pytest 41 | ament_lint_auto 42 | ament_lint_common 43 | 44 | 45 | 46 | ament_cmake 47 | 48 | 49 | -------------------------------------------------------------------------------- /atos_gui/atos_gui/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/atos_gui/atos_gui/__init__.py -------------------------------------------------------------------------------- /atos_gui/atos_gui/configpanel/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/atos_gui/atos_gui/configpanel/__init__.py -------------------------------------------------------------------------------- /atos_gui/atos_gui/controlpanel/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/atos_gui/atos_gui/controlpanel/__init__.py -------------------------------------------------------------------------------- /atos_gui/atos_gui/images/favicon.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/atos_gui/atos_gui/images/favicon.ico -------------------------------------------------------------------------------- /atos_gui/atos_gui/objectpanel/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/atos_gui/atos_gui/objectpanel/__init__.py -------------------------------------------------------------------------------- /atos_gui/launch/gui.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration, PythonExpression 5 | from launch.conditions import IfCondition 6 | 7 | def generate_launch_description(): 8 | 9 | insecure_launch_arg = DeclareLaunchArgument('insecure', default_value='False') 10 | 11 | return LaunchDescription([ 12 | insecure_launch_arg, 13 | Node( 14 | condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('insecure')])), 15 | package='atos_gui', 16 | namespace='atos', 17 | executable='gui', 18 | output='screen', 19 | arguments = ["True"] # Use ssl 20 | ), 21 | Node( 22 | condition=IfCondition(LaunchConfiguration('insecure')), 23 | package='atos_gui', 24 | namespace='atos', 25 | executable='gui', 26 | output='screen', 27 | arguments = ["False"] # Don't use ssl 28 | ) 29 | ]) -------------------------------------------------------------------------------- /atos_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | atos_gui 5 | 1.0.0 6 | This is an GUI for ATOS running in a ROS2 node 7 | Robert Brenick 8 | MPL 2.0 9 | 10 | rclpy 11 | rcl_interfaces 12 | atos_interfaces 13 | 14 | 15 | ament_python 16 | 17 | -------------------------------------------------------------------------------- /atos_gui/requirements.txt: -------------------------------------------------------------------------------- 1 | nicegui==1.4.8 2 | fastapi==0.108.0 3 | -------------------------------------------------------------------------------- /atos_gui/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/atos_gui 3 | [install] 4 | install_scripts=$base/lib/atos_gui -------------------------------------------------------------------------------- /atos_gui/setup.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree as ET 2 | from pathlib import Path 3 | 4 | from setuptools import setup, find_packages 5 | 6 | package_xml = ET.parse('package.xml').getroot() 7 | package_name = package_xml.find('name').text 8 | data = Path('share') / package_name 9 | setup( 10 | name=package_name, 11 | version=package_xml.find('version').text, 12 | packages=find_packages(), 13 | maintainer=package_xml.find('license').text, 14 | maintainer_email=package_xml.find('maintainer').attrib['email'], 15 | license=package_xml.find('license').text, 16 | data_files=[ 17 | (str(data), ['package.xml']), 18 | (str(data / 'launch'), ['launch/gui.py']), 19 | ], 20 | install_requires=['setuptools'], 21 | zip_safe=True, 22 | entry_points={ 23 | 'console_scripts': [ 24 | 'gui = atos_gui.main:main' 25 | ], 26 | }, 27 | ) -------------------------------------------------------------------------------- /conf/Catalogs/Pedestrians/PedestrianCatalog.xosc: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | 11 | 12 |
13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 |
25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 |
37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /conf/Catalogs/Routes/RoutesAtMultiIntersections.xosc: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /conf/README.md: -------------------------------------------------------------------------------- 1 | ## ATOS configuration instructions 2 | 3 | This folder contains various example configuration files which is expected by ATOS to exist in ~/.astazero/ATOS/. 4 | 5 | The requiered configuration files are: 6 | // To be expanded 7 | -------------------------------------------------------------------------------- /conf/conf/params.yaml: -------------------------------------------------------------------------------- 1 | atos: 2 | journal_control: 3 | ros__parameters: 4 | scenario_name: "" 5 | esmini_adapter: 6 | ros__parameters: 7 | timestep: 0.1 8 | open_scenario_gateway: 9 | ros__parameters: 10 | open_scenario_file: "GaragePlanScenario.xosc" 11 | active_object_names: ["1"] 12 | start_delay_ms: 0 13 | object_control: 14 | ros__parameters: 15 | max_missing_heartbeats: 100 16 | transmitter_id: 15 17 | osi_adapter: 18 | ros__parameters: 19 | address: "0.0.0.0" 20 | port: 55555 21 | protocol: "tcp" 22 | frequency: 100 23 | mqtt_bridge: 24 | ros__parameters: 25 | broker: 26 | host: 127.0.0.1 27 | port: 1883 28 | client: 29 | id: "ATOS" 30 | username: "" 31 | password: "" 32 | mqtt2ros: # Object specifying which MQTT topics to map to which ROS topics 33 | mqtt_topics: # Array specifying which ROS topics to bridge 34 | - placeholder1 # The MQTT topic that should be bridged, corresponds to the sub-object in the YAML 35 | placeholder1: 36 | ros_topic: placeholder1 # ROS topic on which corresponding MQTT messages are published 37 | advanced: # Optional settings for each topic 38 | mqtt: 39 | qos: 1 # [0] MQTT QoS value 40 | ros: 41 | queue_size: 1 # [1] ROS publisher queue size 42 | ros2mqtt: # Object specifying which ROS topics to map to which MQTT topics 43 | ros_topics: 44 | - placeholder2 45 | placeholder2: 46 | mqtt_topic: placeholder2 47 | advanced: 48 | mqtt: 49 | qos: 1 50 | ros: 51 | queue_size: 1 52 | monr_relay: 53 | ros__parameters: 54 | host: "127.0.0.1" 55 | port: 51234 56 | trajectorylet_streamer: 57 | ros__parameters: 58 | chunk_duration: 2.0 59 | pointcloud_publisher: 60 | ros__parameters: 61 | pointcloud_files: [""] 62 | integration_testing_handler: 63 | ros__parameters: 64 | scenario_execution: true 65 | back_to_start: 66 | ros__parameters: 67 | turn_radius: 5.0 68 | min_speed: 4.0 69 | max_speed: 12.0 70 | -------------------------------------------------------------------------------- /conf/odr/README.md: -------------------------------------------------------------------------------- 1 | ## ATOS OpenDrive folder 2 | 3 | This folder contains OpenDrive files of various test areas at AstaZero. 4 | 5 | -------------------------------------------------------------------------------- /docker-compose-bridge.yml: -------------------------------------------------------------------------------- 1 | version: "3.2" 2 | networks: 3 | atos-net: 4 | driver: bridge 5 | services: 6 | atos: 7 | image: astazero/atos_docker_env:latest 8 | build: 9 | context: . 10 | dockerfile: ./Dockerfile 11 | ipc: host 12 | privileged: true 13 | stdin_open: true 14 | tty: true 15 | environment: 16 | - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} 17 | volumes: 18 | - ~/.astazero/ATOS/:/root/.astazero/ATOS/ 19 | networks: 20 | - atos-net 21 | extra_hosts: 22 | - "host.docker.internal:host-gateway" 23 | ports: 24 | - "3000:3000" 25 | - "3443:3443" 26 | - "55555:55555" 27 | - "443:443" 28 | - "9090:9090" 29 | - "8765:8765" 30 | command: bash -c "source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" 31 | foxglove-studio: 32 | image: ghcr.io/foxglove/studio:1.76 33 | networks: 34 | - atos-net 35 | ports: 36 | - 8080:8080 37 | volumes: 38 | - ./plugins/foxglove/Map, 3D and control layout.json:/foxglove/default-layout.json -------------------------------------------------------------------------------- /docker-compose-test.yml: -------------------------------------------------------------------------------- 1 | version: "3.2" 2 | networks: 3 | atos-net: 4 | driver: bridge 5 | services: 6 | atos: 7 | image: astazero/atos_docker_env:test 8 | networks: 9 | - atos-net 10 | volumes: 11 | - ./conf/Catalogs/:/root/.astazero/ATOS/Catalogs 12 | - ./conf/conf/:/root/.astazero/ATOS/conf 13 | - ./conf/odr/:/root/.astazero/ATOS/odr 14 | - ./conf/osc/:/root/.astazero/ATOS/osc 15 | command: bash -c "source /root/atos_ws/install/setup.sh ; python3 -m pytest /root/atos_ws/src/atos/scripts/integration_testing/run_scenario_test.py" 16 | isoObject: 17 | image: astazero/iso_object_demo:latest 18 | networks: 19 | - atos-net -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: "3.2" 2 | services: 3 | atos: 4 | image: astazero/atos_docker_env:latest 5 | build: 6 | context: . 7 | dockerfile: ./Dockerfile 8 | ipc: host 9 | privileged: true 10 | stdin_open: true 11 | tty: true 12 | environment: 13 | - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} 14 | volumes: 15 | - ~/.astazero/ATOS/:/root/.astazero/ATOS/ 16 | network_mode: "host" 17 | command: bash -c "source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" 18 | foxglove-studio: 19 | image: ghcr.io/foxglove/studio:1.76 20 | network_mode: "host" 21 | volumes: 22 | - ./plugins/foxglove/Map, 3D and control layout.json:/foxglove/default-layout.json -------------------------------------------------------------------------------- /docs/Contributing/Contributing.md: -------------------------------------------------------------------------------- 1 | # Contributing to this project 2 | 3 | Please take a moment to review this section for an easy and effective contribution process. 4 | 5 | Following these guidelines will help to communicate the issues or pull requests that you 6 | submit in a clear, concise, and effective manner. This increases the chances of getting a quick response from the maintainers, while also increasing the chance of having your code merged into the project. 7 | 8 | This project uses [cppcheck](https://cppcheck.sourceforge.io/) and [clang-tidy](https://clang.llvm.org/extra/clang-tidy/) as linters with complete lists of checks here: [cppcheck list](https://sourceforge.net/p/cppcheck/wiki/ListOfChecks/) and [clang-tidy list](https://clang.llvm.org/extra/clang-tidy/checks/list.html)(only cppcoreguidelines-* and clang-analyzer-* checks.). For formatter, this project uses [clang-format](https://clang.llvm.org/docs/ClangFormat.html) with [LLVM coding standards](https://llvm.org/docs/CodingStandards.html), as well as [doxygen](https://www.doxygen.nl/manual/docblocks.html)-style comments for in-code documentation. 9 | 10 | Releases follow the [semantic versioning](https://semver.org/) scheme. -------------------------------------------------------------------------------- /docs/Contributing/Feature Requests/Feature_requests.md: -------------------------------------------------------------------------------- 1 | ## Feature requests 2 | 3 | Feature requests are welcome. But take a moment to find out whether your idea 4 | fits with the [scope and aims](https://atos.readthedocs.io/en/latest/) of the project. It's up to *you* to make a strong 5 | case to convince the project's developers of the merits of this feature. Please 6 | provide as much detail and context as possible. 7 | -------------------------------------------------------------------------------- /docs/Contributing/Issues/issues.md: -------------------------------------------------------------------------------- 1 | ## Using the issue tracker 2 | 3 | The [issue tracker](https://github.com/RI-SE/ATOS/issues) on the repo page is the preferred channel for [bug reports](#bugs) and feature requests. If you want to submit a pull request that relates to a specific issue or bug, please select the bug in question and use the "create a branch for this issue or link a pull request" option under the development heading to link your pull request to the issue. 4 | 5 | ![image](https://github.com/RI-SE/ATOS/assets/10436623/adb9d512-ab25-4504-aa9d-63c95ba76c85) 6 | 7 | 8 | In general, also please respect the following restrictions: 9 | 10 | * Please **do not** use the issue tracker for personal support requests. [The discussions tab](https://github.com/RI-SE/ATOS/discussions) on the repo can be used for this instead. 11 | 12 | * Please **do not** derail or troll issues. Keep the discussion on topic and 13 | respect the opinions of others. 14 | 15 | 16 | 17 | ## Bug reports 18 | 19 | A bug is an error, flaw or fault in the design of a piece of software or code. 20 | We welcome bug reports, as they are extremely helpful in furthering the development of the project. 21 | 22 | Guidelines for bug reports: 23 | 24 | 1. **Use the GitHub bug search** — check if the bug has already been 25 | reported. 26 | 27 | 2. **Check if the bug has been fixed** — try to reproduce it using the 28 | latest `main` or development branch in the repository. 29 | 30 | 3. **Isolate the problem** — 31 | A good bug report shouldn't leave others needing to chase you up for more 32 | information. Please try to be as detailed as possible in your report. What is 33 | your environment? What steps will reproduce the bug? What OS are you using, which 34 | browser is used for the gui?What would you expect to be the outcome? All these 35 | details will help people to fix any potential bugs. 36 | 37 | The bug report template is present in the bug tab on gihub, a copy of it can be found [here](https://github.com/RI-SE/ATOS/blob/dev/.github/ISSUE_TEMPLATE/bug_report.md) 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /docs/Contributing/Pull Requests/pull_requests.md: -------------------------------------------------------------------------------- 1 | 2 | ## Pull requests 3 | 4 | 5 | **Please ask in the [discussions](https://github.com/RI-SE/ATOS/discussions) tab** before embarking on any significant pull request (e.g. implementing new features, refactoring code, porting to a different language), 6 | otherwise you risk spending a lot of time working on something that the 7 | project's developers might not want to merge into the project. 8 | 9 | Please adhere to the [coding conventions](https://github.com/RI-SE/ATOS/wiki/Coding-Conventions) used throughout the project (indentation, 10 | accurate comments, etc.) and any other requirements (such as test coverage). 11 | 12 | Follow this process if you'd like your work considered for inclusion in the 13 | project. 14 | 15 | **IMPORTANT**: By submitting a patch, you agree to allow the project owner to 16 | license your work under the same license as that used by the project. 17 | 18 | 19 | #### Required information for a pull request 20 | 21 | 22 | * What kind of change does this PR introduce? (Bug fix, feature, docs update, etc.) 23 | 24 | 25 | * What is the current behavior? (You can also link to an open issue here) 26 | 27 | 28 | * What is the new behavior (if this is a feature change)? 29 | 30 | 31 | * Does the PR change the way the software in this project interfaces with internal/external components such as files, other software, etc? 32 | 33 | #### Additional information 34 | * In the case that you feel there is additional information, files images, or something else that will add valuable context, feel free to add anything extra that will help the developers understand the aim and purpose of your pull request.: 35 | 36 | 37 | -------------------------------------------------------------------------------- /docs/Contributing/Testing/testing.md: -------------------------------------------------------------------------------- 1 | # Testing 2 | 3 | ## Unit Tests 4 | Unit tests for C++ code in ATOS are written in gtest. New modules must have unit tests covering at least the core functionality. All unit tests for each module are executed automatically each time a pull request is submitted. 5 | 6 | To run individual unit tests, run the following command from the ATOS build folder in your workspace: 7 | ```bash 8 | cd ~/atos_ws/build/atos 9 | ctest -R 10 | ``` 11 | 12 | To run all unit tests you can just run 13 | ```bash 14 | colcon test --event-handlers console_cohesion+ 15 | ``` 16 | from the root folder of your workspace. 17 | 18 | colcon and cmake switches if test should be built with the BUILD_TESTING flag. This should be set to ON by default. If not you can set it manually with 19 | ```bash 20 | colcon build --cmake-args -DBUILD_TESTING=ON 21 | ``` 22 | 23 | ## Integration testing 24 | The docker compose file docker-compose-test.yml is used to run integration tests towards one or more simulated ISO22133 test objects. These test are used to "smoke test" ATOS and makes sure that no changes 25 | breaks any core functionality. You can start these test with the command 26 | ```bash 27 | docker compose -f docker-compose-test.yml up --abort-on-container-exit 28 | ``` 29 | 30 | If you wish to run these test manually you can find the entry point script at scripts/integration_testing/run_scenario_test.py. Run the test with 31 | ```bash 32 | python3 -m pytest run_scenario_test.py 33 | ``` 34 | !!! note 35 | 36 | Running the integration test manually requires you to set up any necessary test objects yourself! 37 | 38 | ## Sample code 39 | In the Sample Module you will find a sample node and a some tests that triggers message callbacks and service routines. Use this as inspiration when testing your modules. -------------------------------------------------------------------------------- /docs/Images/BackToStart_after.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/Images/BackToStart_after.png -------------------------------------------------------------------------------- /docs/Images/BackToStart_before.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/Images/BackToStart_before.png -------------------------------------------------------------------------------- /docs/Installation/connected.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/Installation/connected.gif -------------------------------------------------------------------------------- /docs/Installation/controlpanel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/Installation/controlpanel.png -------------------------------------------------------------------------------- /docs/Installation/installation.md: -------------------------------------------------------------------------------- 1 | # Instructions 2 | There are two ways of starting ATOS: using the docker image or building from source. The docker image is the easiest way to get started, but if you intend to make changes to ATOS, we recommend building from source with the setup_atos.sh script. 3 | 4 | ## Running with docker 5 | To run ATOS using docker compose, first [install docker](https://docs.docker.com/engine/install/) on your computer. Download the [ATOS repo](https://github.com/RI-SE/ATOS). Then, run the following command from the root repo directory: 6 | ```bash 7 | docker compose up 8 | ``` 9 | or if you are not in the docker group: 10 | ```bash 11 | sudo -E docker compose up 12 | ``` 13 | 14 | Note: If no preexisting .astazero/ATOS folder is found, the docker image will create one. This folder will have root user permissions due to the way the docker image is built. For ease of use, change owner to your own user, run the following command on the host machine: 15 | ```bash 16 | sudo chown -R $USER:$USER ~/.astazero/ATOS/ 17 | ``` 18 | 19 | ## Running natively 20 | ATOS comes with an installation script that automates the installation process. It is intended for use on Ubuntu 22.04. The script will install ROS 2, ATOS dependencies, and ATOS itself. It will also create a workspace and build ATOS. The script can be executed using the following command: 21 | ```bash 22 | ./setup_atos.sh 23 | ``` 24 | 25 | ## Starting ATOS 26 | Launch ATOS 27 | ```bash 28 | ros2 launch atos launch_basic.py insecure:=True 29 | ``` 30 | See the [GUI](../Usage/GUI/foxglove.md) documentation on how to enable secure connections. 31 | 32 | When running ATOS for the first time and the no pre-existing .astazero/ATOS folder is found, ATOS will create some barebone configuration files which you can read more about in the configuration section [here](../Usage/How-to/configuration.md). 33 | -------------------------------------------------------------------------------- /docs/Installation/isoobject_preconnect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/Installation/isoobject_preconnect.png -------------------------------------------------------------------------------- /docs/Installation/quickstart.md: -------------------------------------------------------------------------------- 1 | #### Prerequisites 2 | Make sure you are ready to run ATOS with docker by following the instructions in the [Installation](installation.md) section. 3 | 4 | ------------------------- 5 | 6 | # Quick start 7 | 8 | This section will walk you through the steps of starting ATOS, connecting it with a virtual test object and visualizing the scenario in Foxglove studio. 9 | 10 | * Start ATOS by running either one of the docker commands (or with "ros2 launch ..." if you have installed it locally). In this guide, the docker compose command is used. 11 | 12 | 13 | //ATOS $ docker compose up 14 | (or if you are not in the docker group) 15 | //ATOS $ sudo -E docker compose up 16 | 17 | !!! Warning 18 | Running the default docker compose command will run network traffic insecurely between ATOS and Foxglove. See the [GUI](../Usage/GUI/foxglove.md) documentation on how to enable secure connections. 19 | 20 | 21 | * Make sure the folder ~/.astazero/ATOS/ was created on the host machine. Since it was created by docker you will need to change the owner to your own user. Run the following command on the host machine: 22 | 23 | 24 | sudo chown -R $USER:$USER ~/.astazero/ATOS/ 25 | 26 | 27 | * Start a virtual test object by running 28 | 29 | docker run -it --rm --hostname isoObject --network atos_atos-net --name isoObject astazero/iso_object_demo:latest 30 | 31 | This docker command creates a virtual object which will respond to control messages from ATOS. You can find the source code for the _ISO\_objectDemo_ at the [isoObject repo](https://github.com/RI-SE/isoObject). 32 | 33 | * Open Chrome and go to [http://localhost:8080](http://localhost:8080). 34 | 35 | * Press Open connection -> Foxglove WebSocket. Enter WebSocket URL ws://localhost:8765. Press Open. You should now see ROS topics in the left panel named "Topics". 36 | 37 | * Install the foxglove ATOS extensions from the ATOS repo. Do this by dragging the .foxe files from the folder /{repo_path}/ATOS/plugins/foxglove/ into the Foxglove studio browser window. 38 | 39 | * Press the buttons Init and Connect in the control panel pane. You should now see several trajectories and an object appear in the 3D view. 40 | 41 | * Now press Arm and Start. The object should now follow one of the trajectories. -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 | ## Software architecture 2 | 3 | ![image](res/ATOS_architecture.drawio.png) 4 | 5 | ![image](res/ATOS-UML.png) 6 | 7 | ## Docs 8 | 9 | Docs generated with mkdocs/doxygen and hosted on Read The Docs 10 | 11 | When editing locally, install deps with 12 | "sudo apt install doxygen && pip install -r docs/requirements.txt" 13 | 14 | Then build the doxygen html pages with "doxygen Doxyfile" from the ATOS root folder. 15 | 16 | Then run "mkdocs serve" from the ATOS root folder. 17 | 18 | When adding python deps, add these to requirements.in and then run the "pip-compile" from pip-tools https://pip-tools.readthedocs.io/en/latest/ 19 | -------------------------------------------------------------------------------- /docs/Usage/GUI/controlpanel.md: -------------------------------------------------------------------------------- 1 | # Control Panel 2 | Control Panel is used to run ATOS without any further visualisation. In order to start Control Panel, start ATOS and then open your web browser and go to `ATOS_host_address:3000`, where `ATOS_host_address` is the address to the server that ATOS is hosted on. If you run ATOS on your own computer and want to use Control Panel on the same computer, you would go to `localhost:3000`. 3 | 4 | ## Running over HTTPS/TLS 5 | To connect to a remote ATOS server, you have to accept the following certificates in your browser: `https://hostname.of.atos:3443`, `https://hostname.of.atos:9090`. 6 | You can now follow the steps above to connect to the remote server either using the standalone Control Panel, or as a panel in [FoxGlove Studio](./foxglove.md). 7 | 8 | ## Features 9 | 10 | When opening Control Panel there will be a set of buttons, A state value determined by [ObjectControl](../Modules/ObjectControl.md), and the connection status to the atos gui server. The buttons are: 11 | 12 | - **Init** - Initialise a test, loads things such as the scenario and which objects to use. 13 | - **Connect** - Connects to the objects that were initialised. 14 | - **Arm** - Arms the objects, meaning that they are ready to start the test. 15 | - **Start** - Starts the test. 16 | - **Reset Test** - Reset the loaded test 17 | - **All clear** - Clears the test. 18 | - **Disarm** - Disarms the objects, meaning that they are no longer ready to start the test. 19 | - **Abort** - Stops the test, for instance if something goes wrong or the test needs to be stopped for some reason. -------------------------------------------------------------------------------- /docs/Usage/How-to/configuration.md: -------------------------------------------------------------------------------- 1 | # Configuration 2 | 3 | ## The test directory 4 | After starting ATOS the first time, you will find the test directory `.astazero/ATOS` in your home folder on the host machine (or where you have specified it to be). 5 | This directory contains all configuration settings and journals which are located in the following directories: 6 | 7 | - **Catalogs** 8 | - Explanation: Catalog directory containing various OpenSCENARIO-files with settings and parameters used by the scenario engine esmini. 9 | - Vehicle/VehicleCatalog.xosc - Contains object details about vehicles that can be used in a test. 10 | Mandatory properties are the "ip". The "ip" is the IP-address of ISO22133 interface in the test object. 11 | - **certs** 12 | - Explanation: Directory containing the certificates used by e.g. the web-gui. 13 | - **conf** 14 | - Explanation: Directory containing the configuration files used to configure ATOS, i.e. the ROS parameters located in the file params.yaml. Find more information below. 15 | - **journal** 16 | - Explanation: Directory containing the journal files, i.e. recorded configuration/state/position of each object for the duration of a test. 17 | - **odr** 18 | - Explanation: Directory containing OpenDRIVE-files. 19 | - **osc** 20 | - Explanation: Directory containing the OpenSCENARIO-files. 21 | - **pointclouds** 22 | - Explanation: Directory containing site scans as point clouds. 23 | 24 | 25 | ## Changing ROS parameters 26 | In the `conf`-directory, `params.yaml` is located and sets ROS-parameters for the ATOS modules, which are read when launching ATOS. The parameters are described in their respective module: 27 | 28 | - [ATOSBase](../Modules/ATOSBase.md) 29 | - [EsminiAdapter](../Modules/EsminiAdapter.md) 30 | - [MQTTBridge](../Modules/MQTTBridge.md) 31 | - [ObjectControl](../Modules/ObjectControl.md) 32 | - [OSIAdapter](../Modules/OSIAdapter.md) 33 | - [PointcloudPublisher](../Modules/PointcloudPublisher.md) 34 | - [TrajectoryletStreamer](../Modules/TrajectoryletStreamer.md) 35 | -------------------------------------------------------------------------------- /docs/Usage/How-to/docker_networking.md: -------------------------------------------------------------------------------- 1 | # ATOS and Docker networks 2 | 3 | ## Running isoObject inside docker containers 4 | Starting ATOS with docker compose will create a network named `atos_atos-net`. This network is used to connect the other containers that ATOS uses. Like foxglove-studio and the isoObject demo for example. 5 | 6 | If you wish to run several isoObject demo containers, you can do so by connecting them to the `atos_atos-net` network and make sure they have unique hostnames. You can then use these hostnames in the VehicleCatalog.xosc file in the IP property field. like this 7 | 8 | ```xml 9 | 10 | 11 | 12 | 13 | 14 | ``` 15 | 16 | Then when starting an isoObject container, you can do so by running the following command: 17 | 18 | ```bash 19 | docker run -it --rm --hostname isoObjectA --network atos_atos-net astazero/iso_object_demo:latest 20 | ``` 21 | 22 | This works because the `atos_atos-net` network is a user defined bridge network, which means that they can resolve the hostnames of the containers connected on that network. 23 | 24 | ## Running the isoObject on the host machine 25 | If you wish to run the isoObject on the host machine, you can do so by specifying the hostname of the host machine in the VehicleCatalog file. Like this 26 | 27 | ```xml 28 | 29 | 30 | 31 | 32 | 33 | ``` 34 | 35 | From the docker compose file, the host gateway address is added by these lines 36 | 37 | ```yaml 38 | extra_hosts: 39 | - "host.docker.internal:host-gateway" 40 | ``` -------------------------------------------------------------------------------- /docs/Usage/Modules/BackToStart.md: -------------------------------------------------------------------------------- 1 | # BackToStart 2 | A module for generating an object's return trajectory. 3 | 4 | WARNING: It is up to the operator to determine if the new trajectories are possible to execute or not since the generated trajectories doesn't take other objects' trajectories into consideration. 5 | 6 | ## About the module 7 | This module provides a service that enables resetting test objects to their starting position after a completed test. 8 | The service calculates Williamson turns on both ends of the original trajectory, which if followed by the test object will lead it back to the start of the original trajectory, with a correct heading. 9 | 10 | 1. Init your test: `Init`->`Connect`. 11 | 2. Execute test: `Arm`->`Start`. 12 | 3. Abort the test after it's finished: `Abort`->`Clear Abort` (You will end up in `Connected` state and ready for `Arm`). 13 | 4. Return the test objects to starting position: `Reset Test`->`Arm`->`Start`. 14 | 5. Abort reset after objects have returned: `Abort`->`Clear Abort`. 15 | 6. Reload scenario settings: `Reload Settings`. 16 | 7. You are now ready to execute your test again: return to step 2. 17 | 18 | The service is only allowed in `CONNECTED` state and does not result in a state change. Images below show before pressing `Reset Test` (left) and after (right), the state remains in `CONNECTED`. 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /docs/Usage/Modules/DirectControl.md: -------------------------------------------------------------------------------- 1 | # DirectControl 2 | A module for sending control signals to test equipment. 3 | ## About the module 4 | This module is used for inputting control signals, for example steering and throttle, into ATOS. An application could be to control equipment via a game controller or to pass data from other systems directly to objects. The signal data is not transmitted directly to the objects since other modules are responsible for ensuring correct test object state before sending such data. Instead, the data is re-sent on a ROS2 topic. 5 | -------------------------------------------------------------------------------- /docs/Usage/Modules/EsminiAdapter.md: -------------------------------------------------------------------------------- 1 | #EsminiAdapter 2 | Module for interfacing with the [esmini](https://github.com/esmini/esmini) ScenarioEngine. 3 | ## About the module 4 | This module has the following responsibilities: 5 | * Load an OpenSCENARIO-file and extract static trajectories from it. 6 | * From the associated OpenDRIVE file, extract the test origin and make available to other modules. 7 | * Dynamically, while the test is running, execute actions based on triggers specified in the OpenSCENARIO-file. 8 | Note: some refactoring is needed to divide the responsibilities of this module into two separate modules. 9 | 10 | ## ROS parameters 11 | The following ROS parameters can be set for `EsminiAdapter`: 12 | 13 | - `open_scenario_file` - Name of the OpenSCENARIO-file. The file must end in `.xosc` and be located in the `osc`-directory. 14 | 15 | 16 | ## Example 17 | Load an OpenSCENARIO-file called `MyScenario.xosc`. 18 | 19 | - `open_scenario_file: "MyScenario.xosc"` 20 | 21 | ## Test origin 22 | 23 | The test origin is extracted from the OpenDRIVE file of the scenario. To change the test origin to a different location, change the "geoReference" tag in the OpenDrive file header. 24 | 25 | *Example* 26 | ```xml 27 | 28 |
29 | 30 |
31 | ``` -------------------------------------------------------------------------------- /docs/Usage/Modules/IntegrationTesting.md: -------------------------------------------------------------------------------- 1 | # IntegrationTesting 2 | 3 | ## About the module 4 | `IntegrationTesting` is used for integration tests. The module launches an integration testing handler, which reads from `params.yaml` which integration tests we want to execute. A node for each integration 5 | test is then launched, and the node performs the specified integration tests. An integration tests can for instance be if an object reports all states correctly during a run, or if it follows a trajectory 6 | correctly. This module can be run both offline and through GitHub Actions. 7 | 8 | !!! note 9 | 10 | You can read more about integration testing in the section [Testing](../../Contributing/Testing/testing.md). 11 | 12 | ## ROS parameters 13 | The integration tests are specified in `params.yaml`, and can be set to true of false depending on if we want to run them or not. The following ROS parameters can be set for `IntegrationTesting`: 14 | 15 | ```yaml 16 | integration_testing_handler: 17 | ros__parameters: 18 | scenario_execution: true # set to true or false depending if you want to run this integration test 19 | ``` 20 | -------------------------------------------------------------------------------- /docs/Usage/Modules/JournalControl.md: -------------------------------------------------------------------------------- 1 | # JournalControl 2 | A module for generating output logs for the user. 3 | 4 | ## ROS parameters 5 | The following ROS parameters can be set in the params.yaml file: 6 | 7 | ```yaml 8 | atos: 9 | journal_control: 10 | ros__parameters: 11 | scenario_name: "" # Optional, name of the journal_file 12 | ``` 13 | ## About the module 14 | This module takes data recorded by other modules via the `JournalRecord` functions and merges it into a single ordered file. This data is separate from that which is logged via `RCLCPP_INFO` etc. which is more aimed at developers using the system. The journal data is intended to be used for data relevant to the test execution e.g. position data or triggered events. 15 | 16 | 1. At test arm, JournalControl creates references / "bookmarks" in each other module's journal which are used to keep track of which line marked the point at which the test was armed. 17 | 18 | 2. Once a test is completed JournalControl creates an end bookmark in each journal. 19 | 20 | 3. Weaves together a contiguous record of what happened in the test, based on the content between the start and end bookmarks in each module's journal. 21 | 22 | 4. The record is outputted as a file intended to be downloaded by the system user. 23 | -------------------------------------------------------------------------------- /docs/Usage/Modules/MQTTBridge.md: -------------------------------------------------------------------------------- 1 | # MQTTBridge 2 | 3 | ## About the module 4 | MQTTBridge is a module that allows you to publish V2X data from the ATOS system to a MQTT broker. The module subscribes to the atos/v2x_message and parses the content of this msg 5 | to JSON which is then published over MQTT to a specified topic. 6 | 7 | Note! The module will shut if no broker ip is specified in the params.yaml 8 | ## Integration with EsminiAdapter 9 | The module can be used together with the EsminiAdapter module to trigger V2X while running a OpenScenario file in ATOS. You can find more information how to set this up at [EsminiAdapter](./EsminiAdapter.md). 10 | 11 | ## ROS parameters 12 | The following ROS parameters should be set in the params.yaml file: 13 | 14 | ```yaml 15 | atos: 16 | mqtt_bridge: 17 | ros__parameters: 18 | broker_ip: "" # Required. IP address of the MQTT broker. 19 | pub_client_id: "" # Name of the MQTT client used for publishing. A random number is appended to the name to avoid name collisions. 20 | username: "" # Username if required by the broker. 21 | password: "" # Password if required by the broker. 22 | topic: "" # Topic to publish to. 23 | quality_of_service: "" # QoS level to use for publishing. Can be 0, 1 or 2. 24 | ``` 25 | 26 | ## atos/v2x_message 27 | 28 | ```bash 29 | string message_type # DENM or CAM 30 | string event_id 31 | uint8 cause_code 32 | uint64 detection_time # millis since epoch 33 | int32 altitude # in centimeters 34 | int32 latitude # in microdegrees 35 | int32 longitude # in microdegrees 36 | ``` 37 | 38 | ## Testing the module 39 | 40 | You can test the module by running the following command. 41 | 42 | ```bash 43 | ros2 topic pub -r 1 /atos/v2x_message atos_interfaces/msg/V2x \ 44 | "{message_type: 'DENM', event_id: 'ATOSEvent', cause_code: 12, \ 45 | altitude: 200, detection_time: 1674131259, latitude: 577063000, longitude: 119416860}" 46 | ``` 47 | 48 | This should result in a DENM message being published to the broker and topic specified in params.yaml. -------------------------------------------------------------------------------- /docs/Usage/Modules/OSIAdapter.md: -------------------------------------------------------------------------------- 1 | # OSIAdapter 2 | 3 | ## About the module 4 | `OSIAdapter` is used to translate `MONR`-data into OSI-data, and then send it over either `tcp` or `udp` for all objects used in a test. The following data is currently sent for each object: 5 | 6 | - `Object ID` 7 | - `Position: x, y, z` 8 | - `Velocity: x, y, z` 9 | - `Acceleration, x, y, z` 10 | - `Orientation: yaw` 11 | 12 | ## ROS parameters 13 | The following ROS parameters can be set for `OSIAdapter`: 14 | 15 | - `address` - IP address for client to connect to. 16 | - `port` - Port for client to connect to. 17 | - `protocol` - Which protocol to use, use `"tcp"` or `"udp"`. 18 | - `frequency` - Frequency for sending data, measured in Hz. 19 | 20 | ## Examples 21 | ### Example 1 22 | Using any address on port 55555, using udp and a frequency of 50 Hz. 23 | 24 | - `address: "0.0.0.0"` 25 | - `port: 55555` 26 | - `protocol: "udp"` 27 | - `frequency: 50` 28 | 29 | 30 | ### Example 2 31 | Using localhost on port 12345, using tcp and a frequency of 100 Hz. 32 | 33 | - `address: "127.0.0.1"` 34 | - `port: 12345` 35 | - `protocol: "tcp"` 36 | - `frequency: 100` 37 | -------------------------------------------------------------------------------- /docs/Usage/Modules/ObjectControl.md: -------------------------------------------------------------------------------- 1 | # ObjectControl 2 | A module for controlling test objects according to the ISO 22133 standard. 3 | ## About the module 4 | This module has many responsibilities: 5 | - Establishes and tracks connections with all test objects 6 | - Keeps track of test object states 7 | - Collects position data from objects, and record that data 8 | - Transmit safety heartbeats to the objects 9 | - Configures objects with trajectories and other settings 10 | - Convert input from other modules into or from the ISO 22133 protocol 11 | - Hold the ATOS system state (ISO 22133 control center status) 12 | - Convert positional data into VUT-relative coordinates, if desired 13 | 14 | ## ROS parameters 15 | The following ROS parameters can be set for `ObjectControl`: 16 | 17 | ```yaml 18 | atos: 19 | object_control: 20 | ros__parameters: 21 | max_missing_heartbeats: 1 # The number of position update (MONR) message periods that are allowed to pass since the last received message before an abort signal is sent to all objects. 22 | transmitter_id: 110 # The ISO 22133 transmitted id to be used for ATOS. 23 | ``` 24 | 25 | ## Examples 26 | ### Example 1 27 | At most 3 position updates missing, and transmitter ID set to 175: 28 | ```yaml 29 | atos: 30 | object_control: 31 | ros__parameters: 32 | max_missing_heartbeats: 3 33 | transmitter_id: 175 34 | ``` 35 | -------------------------------------------------------------------------------- /docs/Usage/Modules/PointcloudPublisher.md: -------------------------------------------------------------------------------- 1 | # PointCloudPublisher 2 | 3 | ## About the module 4 | `PointcloudPublisher` is used to publish site scans. This module supports pointclouds that have the file type `.pcd`. If you have a very large pointcloud, it is recommended to downsample it before inputting it into the module. 5 | 6 | ## ROS parameters 7 | The following ROS parameters can be set for `PointcloudPublisher`: 8 | 9 | ```yaml 10 | atos: 11 | pointcloud_publisher: 12 | ros__parameters: 13 | pointcloud_files: ["file1.pcd", "file2.pcd"] # List of one or more pointcloud files to publish. 14 | ``` 15 | 16 | 17 | -------------------------------------------------------------------------------- /docs/Usage/Modules/SampleModule.md: -------------------------------------------------------------------------------- 1 | # SampleModule 2 | 3 | ## About the module 4 | This is a sample module to be used as template for creating new modules. 5 | 6 | ## Features 7 | The sample module is a ros2 node that features some basic publishers and subscribers to various topics. 8 | It also features a TCPServer running in a separate thread. 9 | 10 | ## Usage 11 | In order to compile and launch this module (or any other module created from the template) you need to go to the outermost CMakeLists.txt file in the root of the repository and add the following line: 12 | ``` 13 | set(WITH_MODULE_X ON CACHE BOOL "Enable ModuleX module") 14 | ``` 15 | 16 | 17 | Followed by: 18 | ``` 19 | if(WITH_MODULE_X) 20 | list(APPEND ENABLED_MODULES ModuleX) 21 | endif() 22 | ``` 23 | 24 | Note: When switching ON/OFF certain modules, it might be necessary to remove the CMakeCache.txt file in ~/atos_ws/install/atos/. 25 | 26 | It is also necessary to add the module to a launch file, located in the launch directory. This is done by adding the following line to the list of nodes in the appropriate launch file: 27 | ``` 28 | Node( 29 | package='atos', 30 | namespace='atos', 31 | executable='module_x', 32 | name='module_x', 33 | ) 34 | ``` 35 | 36 | Then you can compile and launch the module by running the following commands: 37 | ``` 38 | MAKEFLAGS=-j5 colcon build --packages-up-to atos 39 | ``` 40 | (tune -j5 to an appropriate number depending on your available RAM memory and CPU cores) 41 | ``` 42 | ros2 launch atos launch_basic.py 43 | ``` -------------------------------------------------------------------------------- /docs/Usage/Modules/TrajectoryletStreamer.md: -------------------------------------------------------------------------------- 1 | # TrajectoryletStreamer 2 | An experimental module for generating piece-wise trajectory "chunks" based on a known trajectory. It is intended for testing dynamic trajectory functionality in other modules with a trajectory that is known beforehand. 3 | 4 | # ROS parameters 5 | The following ROS parameters can be set for `TrajectoryletStreamer`: 6 | 7 | - `chunk_duration` - Length of the chunks to be transmitted, in seconds. The current time is used to find a chunk start point in the trajectory and the chunk length marks the end of that chunk. 8 | -------------------------------------------------------------------------------- /docs/Usage/Summary/summary_of_states.md: -------------------------------------------------------------------------------- 1 | # Summary of ATOS test-states 2 | 3 | Once ATOS is running, the control-panel GUI can be used to issue commands to ATOS. Certain commands are legal only in specific states. States and the legal transitions between states are described in the standard [ISO 22133](https://www.iso.org/standard/78970.html). 4 | 5 | A normal sequence of events is described in the following table: 6 | 7 | | Step | Action | Description | Expected Response | 8 | |------|------|------|------| 9 | | 1 | Initialize the server | Press the `Init` button. | Scenario is loaded and trajectories are generated for each test object. If previously in `Idle` state, ATOS transitions to state: `Initialized` | 10 | | 2 | Connect to all objects. | Press the `Connect` button. | The server will now try to connect to all objects in the scenario by establishing IP-connections. ATOS transitions to the state `Connected`. 11 | | 3 | Arm the scenario. | Press the `Arm` button. | ATOS transitions to state `Armed` to indicate that all objects are armed and ready to start. | 12 | | 4 | Start the scenario. | Press the `Start` button. | The test starts and ATOS transitions to state: `Running`. This means that the test is live, and both physical and virtual test participants, will start moving according to the scenario. | 13 | | 5 | Abort the scenario. | Press the `Abort` button. | ATOS Aborts the scenario and transitions to state `Aborting`. | 14 | | 6 | Reset the system. | Press the `Clear Abort` button. | ATOS transitions to the state `Connected`. | 15 | 16 | -------------------------------------------------------------------------------- /docs/index.md: -------------------------------------------------------------------------------- 1 | # Summary 2 | 3 | 4 | 5 |
6 | 7 | ATOS, short for AV Test Operating System, is a ISO 22133-compliant and ROS2-based control center and coordinator for scenario-based testing of autonomous vehicles. ATOS controls, monitors and coordinates both physical and virtual vehicles and equipment according to scenarios specified in the ASAM OpenSCENARIO® format. It is made for running in real-time and uses GPS time to ensure exact and repeatable execution between runs. 8 | 9 | Source code is available on [GitHub](https://github.com/RI-SE/ATOS) -------------------------------------------------------------------------------- /docs/requirements.in: -------------------------------------------------------------------------------- 1 | jinja2==3.1.3 2 | mkdocs==1.4.2 3 | Markdown==3.3.7 -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | # 2 | # This file is autogenerated by pip-compile with Python 3.8 3 | # by the following command: 4 | # 5 | # pip-compile requirements.in 6 | # 7 | click==8.1.3 8 | # via mkdocs 9 | ghp-import==2.1.0 10 | # via mkdocs 11 | importlib-metadata==6.1.0 12 | # via 13 | # markdown 14 | # mkdocs 15 | jinja2==3.1.3 16 | # via 17 | # -r requirements.in 18 | # mkdocs 19 | markdown==3.3.7 20 | # via 21 | # -r requirements.in 22 | # mkdocs 23 | markupsafe==2.1.2 24 | # via jinja2 25 | mergedeep==1.3.4 26 | # via mkdocs 27 | mkdocs==1.4.2 28 | # via -r requirements.in 29 | packaging==23.0 30 | # via mkdocs 31 | python-dateutil==2.8.2 32 | # via ghp-import 33 | pyyaml==6.0 34 | # via 35 | # mkdocs 36 | # pyyaml-env-tag 37 | pyyaml-env-tag==0.1 38 | # via mkdocs 39 | six==1.16.0 40 | # via python-dateutil 41 | watchdog==3.0.0 42 | # via mkdocs 43 | zipp==3.15.0 44 | # via importlib-metadata 45 | -------------------------------------------------------------------------------- /docs/res/ATOS-UML.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/res/ATOS-UML.png -------------------------------------------------------------------------------- /docs/res/ATOS_architecture.drawio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/res/ATOS_architecture.drawio.png -------------------------------------------------------------------------------- /docs/res/ATOS_icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/docs/res/ATOS_icon.png -------------------------------------------------------------------------------- /mkdocs.yml: -------------------------------------------------------------------------------- 1 | site_name: ATOS Docs 2 | repo_url: https://github.com/RI-SE/ATOS 3 | theme: 4 | name: readthedocs 5 | logo: res/ATOS_icon.png 6 | 7 | markdown_extensions: 8 | - admonition 9 | nav: 10 | - ATOS Doxygen: html/index.html 11 | - About: 12 | - "index.md" 13 | - "About/aboutATOS.md" 14 | - "Usage/Summary/summary_of_states.md" 15 | - Installation: 16 | - "Installation/installation.md" 17 | - Using ATOS: 18 | - "Installation/quickstart.md" 19 | - "Usage/How-to/configuration.md" 20 | - "Usage/How-to/docker_networking.md" 21 | - "Usage/GUI/foxglove.md" 22 | - "Usage/GUI/controlpanel.md" 23 | - Modules: 24 | - "Usage/Modules/BackToStart.md" 25 | - "Usage/Modules/DirectControl.md" 26 | - "Usage/Modules/EsminiAdapter.md" 27 | - "Usage/Modules/OpenScenarioGateway.md" 28 | - "Usage/Modules/IntegrationTesting.md" 29 | - "Usage/Modules/JournalControl.md" 30 | - "Usage/Modules/MQTTBridge.md" 31 | - "Usage/Modules/OSIAdapter.md" 32 | - "Usage/Modules/ObjectControl.md" 33 | - "Usage/Modules/PointcloudPublisher.md" 34 | - "Usage/Modules/SampleModule.md" 35 | - "Usage/Modules/TrajectoryletStreamer.md" 36 | - Contributing: 37 | - "Contributing/Contributing.md" 38 | - "Contributing/Feature Requests/Feature_requests.md" 39 | - "Contributing/Issues/issues.md" 40 | - "Contributing/Pull Requests/pull_requests.md" 41 | - "Contributing/Testing/testing.md" 42 | -------------------------------------------------------------------------------- /plugins/foxglove/astazero.atos_control_panel-0.0.1.foxe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/plugins/foxglove/astazero.atos_control_panel-0.0.1.foxe -------------------------------------------------------------------------------- /plugins/foxglove/astazero.monr_to_pose-0.0.1.foxe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RI-SE/ATOS/60769d377e5d1c9ef96efea27aecbd394216c748/plugins/foxglove/astazero.monr_to_pose-0.0.1.foxe -------------------------------------------------------------------------------- /plugins/rviz2/include/object_monitor_display.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | */ 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include "rviz_common/interaction/forwards.hpp" 11 | #include "rviz_common/properties/color_property.hpp" 12 | #include "rviz_common/properties/enum_property.hpp" 13 | #include "rviz_common/properties/float_property.hpp" 14 | #include "rviz_common/validate_floats.hpp" 15 | #include "rviz_rendering/objects/arrow.hpp" 16 | #include "rviz_rendering/objects/axes.hpp" 17 | #include "rviz_rendering/objects/shape.hpp" 18 | 19 | namespace atos_rviz_plugins { 20 | 21 | class MonitorDisplay : public rviz_common::RosTopicDisplay { 22 | Q_OBJECT 23 | public: 24 | enum Shape { 25 | Arrow, 26 | Axes, 27 | }; 28 | MonitorDisplay(); 29 | 30 | private: 31 | void processMessage(atos_interfaces::msg::Monitor::ConstSharedPtr msg); 32 | void onInitialize() override; 33 | void reset() override; 34 | 35 | void onEnable() override; 36 | void onDisable() override; 37 | 38 | ~MonitorDisplay() override; 39 | 40 | private Q_SLOTS: 41 | 42 | void updateShapeVisibility(); 43 | void updateShapeChoice(); 44 | void updateColorAndAlpha(); 45 | void updateAxisGeometry(); 46 | void updateArrowGeometry(); 47 | 48 | private: 49 | rviz_common::properties::EnumProperty* shape_property_; 50 | 51 | rviz_common::properties::ColorProperty* color_property_; 52 | rviz_common::properties::FloatProperty* alpha_property_; 53 | 54 | rviz_common::properties::FloatProperty* head_radius_property_; 55 | rviz_common::properties::FloatProperty* head_length_property_; 56 | rviz_common::properties::FloatProperty* shaft_radius_property_; 57 | rviz_common::properties::FloatProperty* shaft_length_property_; 58 | 59 | rviz_common::properties::FloatProperty* axes_length_property_; 60 | rviz_common::properties::FloatProperty* axes_radius_property_; 61 | 62 | std::unique_ptr arrow_; 63 | std::unique_ptr axes_; 64 | 65 | bool pose_valid_; 66 | }; 67 | 68 | } // namespace atos_rviz_plugins 69 | -------------------------------------------------------------------------------- /plugins/rviz2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | atos_rviz_plugins 5 | 1.0.0 6 | RVIZ2 plugins for visualizing ATOS communication 7 | lukas 8 | MPL 2.0 9 | 10 | ament_cmake 11 | 12 | qtbase5-dev 13 | rviz_ogre_vendor 14 | 15 | rviz_ogre_vendor 16 | 17 | libqt5-core 18 | libqt5-gui 19 | libqt5-opengl 20 | libqt5-widgets 21 | rviz_ogre_vendor 22 | 23 | rclcpp 24 | rviz_common 25 | rviz_rendering 26 | rviz_default_plugins 27 | atos_interfaces 28 | pluginlib 29 | geometry_msgs 30 | tf2 31 | tf2_geometry_msgs 32 | tf2_ros 33 | 34 | 35 | ament_lint_auto 36 | ament_lint_common 37 | 38 | 39 | ament_cmake 40 | 41 | 42 | -------------------------------------------------------------------------------- /plugins/rviz2/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | Monitoring data from ATOS object 8 | 9 | geometry_msgs/msg/PoseStamped 10 | 11 | -------------------------------------------------------------------------------- /scripts/installation/dependencies.txt: -------------------------------------------------------------------------------- 1 | curl 2 | wget 3 | unzip 4 | libsystemd-dev 5 | libprotobuf-dev 6 | libpaho-mqtt-dev 7 | libpaho-mqttpp-dev 8 | protobuf-compiler 9 | libeigen3-dev 10 | nlohmann-json3-dev 11 | gnupg2 12 | lsb-release 13 | python3-pip 14 | proj-bin 15 | clang 16 | libstdc++-12-dev -------------------------------------------------------------------------------- /scripts/installation/install_atos.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # Check if called without arguments 5 | if [ $# -eq 0 ]; then 6 | echo "Don't call this file directly, use setup_atos.sh instead." 7 | echo "Usage: ./install_deps.sh " 8 | exit 0 9 | fi 10 | 11 | # Take the first argument as the PATH to the ATOS git repo 12 | ATOS_REPO_PATH="$1" 13 | source "${ATOS_REPO_PATH}/scripts/installation/install_functions.sh" 14 | check_command_failed $? "Failed to source ${ATOS_REPO_PATH}/scripts/installation/install_functions.sh" 15 | 16 | ################################################ 17 | ############## Install ATOS #################### 18 | ################################################ 19 | 20 | # Create a workspace dir if it doesn't exist 21 | if [ ! -d "$HOME/atos_ws/src" ]; then 22 | mkdir -p $HOME/atos_ws/src 23 | fi 24 | cd $HOME/atos_ws 25 | 26 | # Update symlinks to atos and atos_interfaces 27 | update_symlink "$ATOS_REPO_PATH" $HOME/atos_ws/src/atos 28 | 29 | # Change directory into the workspace and build, check with the user before continuing 30 | echo "Dependecy installation done and ATOS workspace created." 31 | 32 | # First make sure the submodules are up to date 33 | echo "Updating submodules to make sure they are up to date..." 34 | cd $HOME/atos_ws/src/atos 35 | git submodule update --init --recursive 36 | cd - 37 | 38 | # temporarily cd into the workspace and build with colcon 39 | echo "Building ATOS..." 40 | cd $HOME/atos_ws 41 | source /opt/ros/$ROS_DISTRO/setup.bash 42 | MAKEFLAGS=-j4 colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON 43 | check_command_failed $? "Failed to build ATOS." 44 | cd - 45 | 46 | ##################################### 47 | ###### Configure setup scripts ###### 48 | ##################################### 49 | 50 | atos_setup_script="source $HOME/atos_ws/install/setup." 51 | ros2_setup_script="source /opt/ros/$ROS_DISTRO/setup." 52 | 53 | case "$SHELL" in 54 | */bash) 55 | add_source_line_if_needed $HOME/.bashrc "bash" "${ros2_setup_script}" 56 | add_source_line_if_needed $HOME/.bashrc "bash" "${atos_setup_script}" 57 | ;; 58 | */zsh) 59 | add_source_line_if_needed $HOME/.zshrc "zsh" "${ros2_setup_script}" 60 | add_source_line_if_needed $HOME/.zshrc "zsh" "${atos_setup_script}" 61 | ;; 62 | *) 63 | echo "Unsupported shell detected! Please use either bash or zsh shells to run ATOS" 64 | exit 1 65 | ;; 66 | esac -------------------------------------------------------------------------------- /scripts/installation/install_functions.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Function to get Ubuntu distribution codename 4 | get_ubuntu_codename() { 5 | source /etc/os-release 6 | echo $UBUNTU_CODENAME 7 | } 8 | 9 | # Function that checks if command failed 10 | check_command_failed() { 11 | local exitcode="$1" 12 | local error_message="$2" 13 | 14 | if [ $exitcode -ne 0 ]; then 15 | echo "$error_message" 16 | exit 1 17 | fi 18 | } 19 | 20 | # Function to update symlink if it doesn't point to the correct location 21 | update_symlink() { 22 | local target="$1" 23 | local link_name="$2" 24 | 25 | if [ -L "$link_name" ]; then 26 | current_target="$(readlink "$link_name")" 27 | if [ "$current_target" != "$target" ]; then 28 | rm "$link_name" 29 | ln -s "$target" "$link_name" 30 | echo "Updated symlink $link_name to $target because it pointed to $current_target previously." 31 | fi 32 | else 33 | ln -s "$target" "$link_name" 34 | echo "Created symlink $link_name to $target" 35 | fi 36 | } 37 | 38 | # Function to add the setup.bash source line if it doesn't exist 39 | add_source_line_if_needed() { 40 | local file="$1" 41 | local shell_type="$2" 42 | local source_line="$3$shell_type" 43 | 44 | if [ ! grep -qF "$source_line" "$file" ]; then 45 | # Ask the user if they want to add the source line. 46 | # First check for noninteractive shell with DEBAIN_FRONTEND=noninteractive 47 | if [ -z "$DEBIAN_FRONTEND" && ! -z $GITHUB_ACTION ]; then 48 | echo "Do you want to add the following line to your $shell_type config file $file:" 49 | echo "$source_line" 50 | echo "y/n" 51 | read -r answer 52 | if [ "$answer" != "${answer#[Yy]}" ]; then 53 | echo "# Line below added by ATOS setup script" >> "$file" 54 | echo "$source_line" >> "$file" 55 | fi 56 | else 57 | echo "# Line below added by ATOS setup script" >> "$file" 58 | echo "$source_line" >> "$file" 59 | fi 60 | fi 61 | } -------------------------------------------------------------------------------- /scripts/installation/requirements.txt: -------------------------------------------------------------------------------- 1 | pre-commit 2 | pyOpenSSL >= 23.2.0 3 | numpy<2.0 4 | scenariogeneration -------------------------------------------------------------------------------- /scripts/openscenario/README.md: -------------------------------------------------------------------------------- 1 | ## ATOS OpenScenario generation script 2 | 3 | The script located in this folder produced the OpenScenario file called GaragePlanScenario.xosc in the conf/osc folder of this repository. 4 | The script is included here as a reference of one way to generate OpenScenario files for ATOS. 5 | 6 | ### How to run the script: 7 | 8 | Install the required python packages: 9 | ```bash 10 | pip install -r requirements.txt 11 | ``` 12 | 13 | run the script: 14 | ```bash 15 | python garage_plan_test_scenario.py 16 | ``` -------------------------------------------------------------------------------- /scripts/openscenario/requirements.txt: -------------------------------------------------------------------------------- 1 | scenariogeneration -------------------------------------------------------------------------------- /setup_atos.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ##################################### 4 | ###### Pre-installation checks ###### 5 | ##################################### 6 | 7 | source "scripts/installation/install_functions.sh" 8 | 9 | # Get this file location 10 | REPO_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" &> /dev/null && pwd)" 11 | 12 | # Check if running on Ubuntu 13 | if ! grep -q "Ubuntu" /etc/os-release; then 14 | echo "This script is designed for Ubuntu systems only." 15 | exit 1 16 | fi 17 | 18 | # Set ROS_DISTRO based on Ubuntu distribution 19 | case "$(get_ubuntu_codename)" in 20 | "focal") 21 | ;; 22 | "jammy") 23 | ;; 24 | *) 25 | echo "Unsupported Ubuntu distribution. Only 20.04 (focal) and 22.04 (jammy) are supported." 26 | exit 1 27 | ;; 28 | esac 29 | 30 | # Add -h/--help option 31 | if [ "$1" == "-h" ] || [ "$1" == "--help" ]; then 32 | echo "Usage: ./setup_atos.sh [single option]" 33 | echo "This script will install all necessary dependencies, setup the ROS workspace at ~/atos_ws and install ATOS. Please open and inspect this script for further details." 34 | echo "Options:" 35 | echo " -h, --help Show this help message and exit" 36 | echo " -r Reinstall dependencies" 37 | exit 0 38 | fi 39 | 40 | # Only install dependencies 41 | if [ "$1" == "-r" ]; then 42 | REINSTALL_DEPS="-r" 43 | fi 44 | 45 | ####################################### 46 | ###### Install ATOS dependencies ###### 47 | ####################################### 48 | echo "Installing ATOS dependencies..." 49 | ${REPO_DIR}/scripts/installation/install_deps.sh ${REPO_DIR} ${REINSTALL_DEPS} 50 | 51 | if [ $? -ne 0 ]; then 52 | echo "Failed to install dependencies." 53 | exit 1 54 | fi 55 | ######################################## 56 | ###### Start installation of ATOS ###### 57 | ######################################## 58 | if ! [ -z "$REINSTALL_DEPS" ]; then 59 | echo "ATOS installation skipped." 60 | exit 0 61 | fi 62 | 63 | echo "Installing ATOS..." 64 | ${REPO_DIR}/scripts/installation/install_atos.sh ${REPO_DIR} 65 | 66 | if [ $? -ne 0 ]; then 67 | echo "Failed to install ATOS." 68 | exit 1 69 | fi 70 | 71 | echo "ATOS build and setup is complete. Please restart your terminal to complete the installation." 72 | echo "Please see the documentation for further details: https://atos.readthedocs.io/en/latest/" 73 | --------------------------------------------------------------------------------