From cf4a64bf66be0e2f5cd652dbf8388e3099fc334d Mon Sep 17 00:00:00 2001 From: iamaisim-team Date: Thu, 2 Jul 2026 16:15:10 -0300 Subject: [PATCH 1/2] Client cpp with all features --------- Co-authored-by: aurebidart Co-authored-by: Lucas Scheinkerman Co-authored-by: Marina Gloc Co-authored-by: jonyMarino --- .github/workflows/test_cpp_client.yml | 257 +++ .gitignore | 9 +- CMakeLists.txt | 96 +- build.cmd | 128 +- build_linux.mk | 53 +- build_windows.mk | 69 +- client/cpp/AirSimClientDLL/AirSimClient.rc | Bin 4724 -> 0 bytes .../AirSimClientDLL/AirSimClientDLL_2019.sln | 65 - .../AirSimClientDLL_2019.vcxproj | 331 ---- .../AirSimClientDLL_2019.vcxproj.filters | 143 -- .../AirSimClientDLL/AirSimClientDLL_2022.sln | 65 - .../AirSimClientDLL_2022.vcxproj | 331 ---- .../AirSimClientDLL_2022.vcxproj.filters | 143 -- .../Include/AirSimClient/ASCDecl.h | 14 - .../Include/AirSimClient/Drone.h | 150 -- .../Include/AirSimClient/World.h | 156 -- client/cpp/AirSimClientDLL/dllmain.cpp | 24 - client/cpp/AirSimClientDLL/framework.h | 10 - client/cpp/AirSimClientDLL/pch.cpp | 10 - client/cpp/AirSimClientDLL/resource.h | 19 - client/cpp/AirSimClientDLL/src/Drone.cpp | 892 --------- .../AirSimMessage_2019.vcxproj | 280 --- .../AirSimMessage_2019.vcxproj.filters | 69 - .../AirSimMessage_2022.vcxproj | 280 --- .../AirSimMessage_2022.vcxproj.filters | 69 - client/cpp/AirSimMessageLib/framework.h | 3 - client/cpp/AirSimMessageLib/pch.cpp | 10 - client/cpp/CMakeLists.txt | 589 ++++++ client/cpp/NNGI/framework.h | 3 +- client/cpp/NNGI/include/NNGI/NNGI.h | 7 +- client/cpp/NNGI/include/NNGI/NNGIDecl.h | 15 +- .../ProjectAirSimMessage}/image_message.hpp | 0 .../include/ProjectAirSimMessage}/jsonc.hpp | 0 .../pose_stamped_message.hpp | 0 .../ProjectAirSimMessage}/request_message.hpp | 0 .../response_message.hpp | 1 + .../pch.h | 3 - .../src/image_message.cpp | 0 .../src/jsonc.cpp | 0 .../src/message.cpp | 0 .../src/pose_stamped_message.cpp | 0 .../src/request_message.cpp | 0 .../src/response_failure_message.cpp | 0 .../src/response_message.cpp | 0 .../src/response_success_message.cpp | 0 .../Include/ProjectAirsimClient/ASCDecl.h | 21 + .../ProjectAirsimClient}/AsyncResult.h | 0 .../Include/ProjectAirsimClient}/Client.h | 8 +- .../Include/ProjectAirsimClient}/Common.h | 0 .../Include/ProjectAirsimClient/Drone.h | 251 +++ .../Include/ProjectAirsimClient/EnvActor.h | 81 + .../ProjectAirsimClient}/IRefCounted.h | 0 .../Include/ProjectAirsimClient}/Log.h | 0 .../Include/ProjectAirsimClient}/Message.h | 0 .../ProjectAirsimClient.h} | 3 + .../Include/ProjectAirsimClient/Rover.h | 84 + .../ProjectAirsimClient/StaticSensorActor.h | 59 + .../Include/ProjectAirsimClient}/Status.h | 1 + .../Include/ProjectAirsimClient}/Types.h | 14 + .../Include/ProjectAirsimClient/World.h | 312 +++ .../pch.h | 15 +- .../src/AsyncResult.cpp | 0 .../src/AsyncResultInternal.cpp | 2 +- .../src/AsyncResultInternal.h | 3 +- .../src/Client.cpp | 140 +- .../cpp/ProjectAirsimClientLib/src/Drone.cpp | 1676 +++++++++++++++++ .../ProjectAirsimClientLib/src/EnvActor.cpp | 259 +++ .../src/ICommChannel.cpp | 0 .../src/ICommChannel.h | 0 .../src/IMessageBuffer.h | 0 .../src/JSONUtils.h | 17 + .../src/Log.cpp | 4 +- .../src/Message.cpp | 0 .../src/ProjectAirsimClient.cpp} | 2 +- .../src/RefCounted.cpp | 0 .../src/RefCounted.h | 0 .../cpp/ProjectAirsimClientLib/src/Rover.cpp | 365 ++++ .../src/StaticSensorActor.cpp | 130 ++ .../src/Status.cpp | 0 .../src/Topics.cpp | 2 +- .../src/Topics.h | 1 + .../src/Types.cpp | 2 + .../src/Utils.cpp | 30 +- .../src/Utils.h | 13 + .../src/World.cpp | 698 ++++++- .../src/airsim_proxies/math_utils.cpp | 2 +- client/cpp/README.md | 264 +++ .../cpp/docs/README_CPP_CLIENT_SCENARIOS.md | 138 ++ .../CppClientScenarios/CppClientScenarios.cpp | 374 ++++ .../HelloDrone/HelloDrone.cpp | 79 +- .../HelloDrone/HelloDrone.vcxproj | 26 +- .../example_user_apps/HelloDrone/README.md | 25 +- .../UserEnvActorScenario.cpp | 224 +++ .../UserLidarScenario/UserLidarScenario.cpp | 242 +++ .../UserRadarScenario/UserRadarScenario.cpp | 257 +++ .../UserRoverScenario/UserRoverScenario.cpp | 260 +++ .../UserScenarioTemplate.cpp | 140 ++ .../UserStaticSensorScenario.cpp | 144 ++ .../ProjectAirSimCppClientConfig.cmake.in | 38 + client/cpp/packaging/README.md.in | 65 + .../scripts/run_cpp_client_scenario_basic.sh | 3 + .../run_cpp_client_scenario_battery.sh | 3 + .../run_cpp_client_scenario_env_actor.sh | 8 + .../scripts/run_cpp_client_scenario_lidar.sh | 55 + .../scripts/run_cpp_client_scenario_radar.sh | 55 + .../run_cpp_client_scenario_sensors.sh | 3 + .../run_cpp_client_scenario_static_sensor.sh | 9 + .../run_cpp_client_scenario_two_drones.sh | 3 + .../scripts/run_cpp_client_scenario_wind.sh | 3 + .../cpp/scripts/run_cpp_client_scenarios.sh | 159 ++ client/cpp/scripts/run_cpp_user_scenario.sh | 90 + .../cpp/tests/CppClientIntegrationTests.cpp | 38 + client/cpp/tests/CppClientUnitTests.cpp | 1554 +++++++++++++++ client/cpp/tests/FakeNNGI.cpp | 170 ++ client/cpp/tests/FakeNNGI.h | 19 + client/cpp/tests/README.md | 67 + core_sim/src/CMakeLists.txt | 2 +- core_sim/src/client_authorization_linux.cpp | 3 +- core_sim/src/sensors/camera.cpp | 29 +- core_sim/src/topic_manager.cpp | 4 +- core_sim/test/CMakeLists.txt | 2 +- docs/client_setup.md | 42 +- docs/cpp_client.md | 293 +++ docs/development/use_source.md | 7 +- docs/index.md | 3 +- physics/test/CMakeLists.txt | 2 +- rendering/scene/test/CMakeLists.txt | 2 +- samples/CMakeLists.txt | 3 +- samples/standalone_sim/CMakeLists.txt | 3 +- .../multirotor_api/test/CMakeLists.txt | 2 +- vehicle_apis/rover_api/test/CMakeLists.txt | 2 +- 131 files changed, 10174 insertions(+), 3229 deletions(-) create mode 100644 .github/workflows/test_cpp_client.yml delete mode 100644 client/cpp/AirSimClientDLL/AirSimClient.rc delete mode 100644 client/cpp/AirSimClientDLL/AirSimClientDLL_2019.sln delete mode 100644 client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj delete mode 100644 client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj.filters delete mode 100644 client/cpp/AirSimClientDLL/AirSimClientDLL_2022.sln delete mode 100644 client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj delete mode 100644 client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj.filters delete mode 100644 client/cpp/AirSimClientDLL/Include/AirSimClient/ASCDecl.h delete mode 100644 client/cpp/AirSimClientDLL/Include/AirSimClient/Drone.h delete mode 100644 client/cpp/AirSimClientDLL/Include/AirSimClient/World.h delete mode 100644 client/cpp/AirSimClientDLL/dllmain.cpp delete mode 100644 client/cpp/AirSimClientDLL/framework.h delete mode 100644 client/cpp/AirSimClientDLL/pch.cpp delete mode 100644 client/cpp/AirSimClientDLL/resource.h delete mode 100644 client/cpp/AirSimClientDLL/src/Drone.cpp delete mode 100644 client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj delete mode 100644 client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj.filters delete mode 100644 client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj delete mode 100644 client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj.filters delete mode 100644 client/cpp/AirSimMessageLib/framework.h delete mode 100644 client/cpp/AirSimMessageLib/pch.cpp create mode 100644 client/cpp/CMakeLists.txt rename client/cpp/{AirSimMessageLib/include/AirSimMessage => ProjectAirSimMessageLib/include/ProjectAirSimMessage}/image_message.hpp (100%) rename client/cpp/{AirSimMessageLib/include/AirSimMessage => ProjectAirSimMessageLib/include/ProjectAirSimMessage}/jsonc.hpp (100%) rename client/cpp/{AirSimMessageLib/include/AirSimMessage => ProjectAirSimMessageLib/include/ProjectAirSimMessage}/pose_stamped_message.hpp (100%) rename client/cpp/{AirSimMessageLib/include/AirSimMessage => ProjectAirSimMessageLib/include/ProjectAirSimMessage}/request_message.hpp (100%) rename client/cpp/{AirSimMessageLib/include/AirSimMessage => ProjectAirSimMessageLib/include/ProjectAirSimMessage}/response_message.hpp (98%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/pch.h (93%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/image_message.cpp (100%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/jsonc.cpp (100%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/message.cpp (100%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/pose_stamped_message.cpp (100%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/request_message.cpp (100%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/response_failure_message.cpp (100%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/response_message.cpp (100%) rename client/cpp/{AirSimMessageLib => ProjectAirSimMessageLib}/src/response_success_message.cpp (100%) create mode 100644 client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ASCDecl.h rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/AsyncResult.h (100%) rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/Client.h (86%) rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/Common.h (100%) create mode 100644 client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Drone.h create mode 100644 client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/EnvActor.h rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/IRefCounted.h (100%) rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/Log.h (100%) rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/Message.h (100%) rename client/cpp/{AirSimClientDLL/Include/AirSimClient/AirSimClient.h => ProjectAirsimClientLib/Include/ProjectAirsimClient/ProjectAirsimClient.h} (92%) create mode 100644 client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Rover.h create mode 100644 client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/StaticSensorActor.h rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/Status.h (98%) rename client/cpp/{AirSimClientDLL/Include/AirSimClient => ProjectAirsimClientLib/Include/ProjectAirsimClient}/Types.h (91%) create mode 100644 client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/World.h rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/pch.h (87%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/AsyncResult.cpp (100%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/AsyncResultInternal.cpp (99%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/AsyncResultInternal.h (99%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Client.cpp (87%) create mode 100644 client/cpp/ProjectAirsimClientLib/src/Drone.cpp create mode 100644 client/cpp/ProjectAirsimClientLib/src/EnvActor.cpp rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/ICommChannel.cpp (100%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/ICommChannel.h (100%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/IMessageBuffer.h (100%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/JSONUtils.h (80%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Log.cpp (98%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Message.cpp (100%) rename client/cpp/{AirSimClientDLL/src/AirSimClient.cpp => ProjectAirsimClientLib/src/ProjectAirsimClient.cpp} (91%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/RefCounted.cpp (100%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/RefCounted.h (100%) create mode 100644 client/cpp/ProjectAirsimClientLib/src/Rover.cpp create mode 100644 client/cpp/ProjectAirsimClientLib/src/StaticSensorActor.cpp rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Status.cpp (100%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Topics.cpp (99%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Topics.h (99%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Types.cpp (97%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Utils.cpp (90%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/Utils.h (89%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/World.cpp (63%) rename client/cpp/{AirSimClientDLL => ProjectAirsimClientLib}/src/airsim_proxies/math_utils.cpp (76%) create mode 100644 client/cpp/README.md create mode 100644 client/cpp/docs/README_CPP_CLIENT_SCENARIOS.md create mode 100644 client/cpp/example_user_apps/CppClientScenarios/CppClientScenarios.cpp create mode 100644 client/cpp/example_user_apps/UserEnvActorScenario/UserEnvActorScenario.cpp create mode 100644 client/cpp/example_user_apps/UserLidarScenario/UserLidarScenario.cpp create mode 100644 client/cpp/example_user_apps/UserRadarScenario/UserRadarScenario.cpp create mode 100644 client/cpp/example_user_apps/UserRoverScenario/UserRoverScenario.cpp create mode 100644 client/cpp/example_user_apps/UserScenarioTemplate/UserScenarioTemplate.cpp create mode 100644 client/cpp/example_user_apps/UserStaticSensorScenario/UserStaticSensorScenario.cpp create mode 100644 client/cpp/packaging/ProjectAirSimCppClientConfig.cmake.in create mode 100644 client/cpp/packaging/README.md.in create mode 100755 client/cpp/scripts/run_cpp_client_scenario_basic.sh create mode 100755 client/cpp/scripts/run_cpp_client_scenario_battery.sh create mode 100755 client/cpp/scripts/run_cpp_client_scenario_env_actor.sh create mode 100644 client/cpp/scripts/run_cpp_client_scenario_lidar.sh create mode 100644 client/cpp/scripts/run_cpp_client_scenario_radar.sh create mode 100755 client/cpp/scripts/run_cpp_client_scenario_sensors.sh create mode 100755 client/cpp/scripts/run_cpp_client_scenario_static_sensor.sh create mode 100755 client/cpp/scripts/run_cpp_client_scenario_two_drones.sh create mode 100755 client/cpp/scripts/run_cpp_client_scenario_wind.sh create mode 100755 client/cpp/scripts/run_cpp_client_scenarios.sh create mode 100755 client/cpp/scripts/run_cpp_user_scenario.sh create mode 100644 client/cpp/tests/CppClientIntegrationTests.cpp create mode 100644 client/cpp/tests/CppClientUnitTests.cpp create mode 100644 client/cpp/tests/FakeNNGI.cpp create mode 100644 client/cpp/tests/FakeNNGI.h create mode 100644 client/cpp/tests/README.md create mode 100644 docs/cpp_client.md diff --git a/.github/workflows/test_cpp_client.yml b/.github/workflows/test_cpp_client.yml new file mode 100644 index 00000000..a6df67ed --- /dev/null +++ b/.github/workflows/test_cpp_client.yml @@ -0,0 +1,257 @@ +name: C++ Client Build and Package + +"on": + workflow_dispatch: + pull_request: + branches: + - main + paths: + - ".github/workflows/test_cpp_client.yml" + - "client/cpp/**" + - "build.cmd" + - "build.sh" + - "build_windows.mk" + - "build_linux.mk" + push: + branches: + - main + paths: + - ".github/workflows/test_cpp_client.yml" + - "client/cpp/**" + - "build.cmd" + - "build.sh" + - "build_windows.mk" + - "build_linux.mk" + +permissions: + contents: read + +jobs: + windows: + name: Windows package + runs-on: windows-2022 + timeout-minutes: 90 + + steps: + - uses: actions/checkout@v4 + + - name: Enable Developer Command Prompt + uses: ilammy/msvc-dev-cmd@v1 + + - name: Configure + shell: cmd + run: > + cmake -S client/cpp + -B client/cpp/build_windows/Release + -G Ninja + -DCMAKE_BUILD_TYPE=Release + + - name: Build + shell: cmd + run: cmake --build client/cpp/build_windows/Release --config Release --parallel + + - name: Package + shell: cmd + run: cmake --build client/cpp/build_windows/Release --target package --config Release + + - name: Extract package + shell: pwsh + run: | + $archive = Get-ChildItem packages/projectairsim_cpp_client -Filter "ProjectAirSimCppClient-*-win64.zip" | Select-Object -First 1 + if (-not $archive) { + throw "ProjectAirSim C++ client package was not created." + } + + New-Item -ItemType Directory -Force _cpp_client_ci/package | Out-Null + Expand-Archive -Path $archive.FullName -DestinationPath _cpp_client_ci/package -Force + + $packageRoot = Get-ChildItem _cpp_client_ci/package -Directory | Select-Object -First 1 + if (-not $packageRoot) { + throw "ProjectAirSim C++ client package did not contain a top-level directory." + } + + $packageConfig = Get-ChildItem $packageRoot.FullName -Recurse -Filter "ProjectAirSimCppClientConfig.cmake" | Select-Object -First 1 + if (-not $packageConfig) { + Get-ChildItem $packageRoot.FullName -Recurse | Select-Object -ExpandProperty FullName + throw "ProjectAirSim C++ client CMake config was not found in the package." + } + + "PACKAGE_ROOT=$($packageRoot.FullName)" | Out-File -FilePath $env:GITHUB_ENV -Append + "PACKAGE_CONFIG_DIR=$($packageConfig.Directory.FullName)" | Out-File -FilePath $env:GITHUB_ENV -Append + + - name: Smoke-test CMake package + shell: pwsh + run: | + $packageConfig = Get-ChildItem _cpp_client_ci/package -Recurse -Filter "ProjectAirSimCppClientConfig.cmake" | Select-Object -First 1 + if (-not $packageConfig) { + Get-ChildItem _cpp_client_ci/package -Recurse | Select-Object -ExpandProperty FullName + throw "ProjectAirSim C++ client CMake config was not found before smoke test." + } + + $packageConfigDir = $packageConfig.Directory.FullName + Write-Host "Using ProjectAirSimCppClient_DIR=$packageConfigDir" + + New-Item -ItemType Directory -Force _cpp_client_ci/consumer | Out-Null + + $cmakeLists = @' + cmake_minimum_required(VERSION 3.20) + project(ProjectAirSimCppClientConsumer LANGUAGES CXX) + + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + + find_package(ProjectAirSimCppClient REQUIRED) + + add_executable(consumer main.cpp) + target_link_libraries(consumer PRIVATE ProjectAirSim::Client) + '@ + + $cmakeLists | Set-Content _cpp_client_ci/consumer/CMakeLists.txt + $mainSource = @' + #include + + int main() { + return microsoft::projectairsim::client::Client::GetVersion() == nullptr; + } + '@ + + $mainSource | Set-Content _cpp_client_ci/consumer/main.cpp + + cmake -S _cpp_client_ci/consumer ` + -B _cpp_client_ci/consumer/build ` + -G Ninja ` + -DCMAKE_BUILD_TYPE=Release ` + -DProjectAirSimCppClient_DIR="$packageConfigDir" ` + -DCMAKE_PREFIX_PATH="$packageConfigDir" + + cmake --build _cpp_client_ci/consumer/build --config Release --parallel + + - name: Upload package + uses: actions/upload-artifact@v4 + with: + name: projectairsim-cpp-client-win64 + path: | + packages/projectairsim_cpp_client/ProjectAirSimCppClient-*-win64.zip + packages/projectairsim_cpp_client/ProjectAirSimCppClient-*-win64.zip.sha256 + if-no-files-found: error + + linux: + name: Linux package (${{ matrix.os }}) + runs-on: ${{ matrix.os }} + timeout-minutes: 90 + + strategy: + fail-fast: false + matrix: + os: + - ubuntu-22.04 + - ubuntu-24.04 + + steps: + - uses: actions/checkout@v4 + + - name: Install Linux Prerequisites + shell: bash + run: | + sudo apt-get update + sudo apt-get -y install --no-install-recommends \ + build-essential cmake ninja-build + + - name: Configure + shell: bash + run: | + cmake -S client/cpp \ + -B client/cpp/build_linux/Release \ + -G Ninja \ + -DCMAKE_BUILD_TYPE=Release + + - name: Build + shell: bash + run: cmake --build client/cpp/build_linux/Release --config Release --parallel + + - name: Package + shell: bash + run: cmake --build client/cpp/build_linux/Release --target package --config Release + + - name: Extract package + shell: bash + run: | + archive="$(find packages/projectairsim_cpp_client -maxdepth 1 -name 'ProjectAirSimCppClient-*-linux64.tar.gz' -print -quit)" + if [ -z "$archive" ]; then + echo "ProjectAirSim C++ client package was not created." >&2 + exit 1 + fi + + mkdir -p _cpp_client_ci/package + tar -xzf "$archive" -C _cpp_client_ci/package + + package_root="$(find _cpp_client_ci/package -mindepth 1 -maxdepth 1 -type d -print -quit)" + if [ -z "$package_root" ]; then + echo "ProjectAirSim C++ client package did not contain a top-level directory." >&2 + exit 1 + fi + + package_config="$(find "$package_root" -name 'ProjectAirSimCppClientConfig.cmake' -print -quit)" + if [ -z "$package_config" ]; then + find "$package_root" -maxdepth 5 -print + echo "ProjectAirSim C++ client CMake config was not found in the package." >&2 + exit 1 + fi + + echo "PACKAGE_ROOT=$package_root" >> "$GITHUB_ENV" + echo "PACKAGE_CONFIG_DIR=$(dirname "$package_config")" >> "$GITHUB_ENV" + + - name: Smoke-test CMake package + shell: bash + run: | + package_config="$(find _cpp_client_ci/package -name 'ProjectAirSimCppClientConfig.cmake' -print -quit)" + if [ -z "$package_config" ]; then + find _cpp_client_ci/package -maxdepth 6 -print + echo "ProjectAirSim C++ client CMake config was not found before smoke test." >&2 + exit 1 + fi + + package_config="$(realpath "$package_config")" + package_config_dir="$(dirname "$package_config")" + echo "Using ProjectAirSimCppClient_DIR=$package_config_dir" + + mkdir -p _cpp_client_ci/consumer + + cat > _cpp_client_ci/consumer/CMakeLists.txt <<'EOF' + cmake_minimum_required(VERSION 3.20) + project(ProjectAirSimCppClientConsumer LANGUAGES CXX) + + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + + find_package(ProjectAirSimCppClient REQUIRED) + + add_executable(consumer main.cpp) + target_link_libraries(consumer PRIVATE ProjectAirSim::Client) + EOF + + cat > _cpp_client_ci/consumer/main.cpp <<'EOF' + #include + + int main() { + return microsoft::projectairsim::client::Client::GetVersion() == nullptr; + } + EOF + + cmake -S _cpp_client_ci/consumer \ + -B _cpp_client_ci/consumer/build \ + -G Ninja \ + -DCMAKE_BUILD_TYPE=Release \ + -DProjectAirSimCppClient_DIR="$package_config_dir" \ + -DCMAKE_PREFIX_PATH="$package_config_dir" + + cmake --build _cpp_client_ci/consumer/build --config Release --parallel + + - name: Upload package + uses: actions/upload-artifact@v4 + with: + name: projectairsim-cpp-client-${{ matrix.os }}-linux64 + path: | + packages/projectairsim_cpp_client/ProjectAirSimCppClient-*-linux64.tar.gz + packages/projectairsim_cpp_client/ProjectAirSimCppClient-*-linux64.tar.gz.sha256 + if-no-files-found: error diff --git a/.gitignore b/.gitignore index e0a4e76e..69471d29 100644 --- a/.gitignore +++ b/.gitignore @@ -24,6 +24,10 @@ bld/ [Bb]uild/ [Tt]esting/ +# Project AirSim C++ client local Linux build artifacts +client/cpp/build_linux/ +client/cpp/build_local/ + # Visual Studio 2015/2017 cache/options directory .vs/ # Uncomment if you have tasks that create the project's static files in wwwroot @@ -345,6 +349,9 @@ unreal/**/Samples # VS Code projects *.code-workspace +# Local developer tools +.tools/ + # VS2019 WSL Intellisense cache **/out @@ -379,4 +386,4 @@ env/ **/slprj *.slxc -venv \ No newline at end of file +venv diff --git a/CMakeLists.txt b/CMakeLists.txt index 0607cb66..82fe04a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -249,12 +249,10 @@ FetchContent_Declare( GIT_TAG cpp-6.1.0 ) FetchContent_GetProperties(msgpack) +add_definitions(-DMSGPACK_NO_BOOST) if(NOT msgpack_POPULATED) message("Checking if [msgpack] dependency needs to be fetched...") FetchContent_Populate(msgpack) - add_definitions(-DMSGPACK_NO_BOOST) - set(MSGPACK_INCLUDE_DIR ${msgpack_SOURCE_DIR}/include) - message("MSGPACK_INCLUDE_DIR set to ${MSGPACK_INCLUDE_DIR}") # Note: msgpack is only used internally by sim libs, so no need to package it with other sim libs # Copy msgpack headers to Matlab physics and control S-function folders for building in Matlab: @@ -264,25 +262,42 @@ if(NOT msgpack_POPULATED) message("Packaging [msgpack] headers to ${MATLAB_CONTROL_DIR}/msgpack") file(COPY ${msgpack_SOURCE_DIR}/include/ DESTINATION ${MATLAB_CONTROL_DIR}/_deps/msgpack/include) endif() +set(MSGPACK_INCLUDE_DIR ${msgpack_SOURCE_DIR}/include) +message("MSGPACK_INCLUDE_DIR set to ${MSGPACK_INCLUDE_DIR}") # Set up dependency: Eigen3 # Using latest stable release as of from 08/18/21 -FetchContent_Declare( - Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.0 -) -FetchContent_GetProperties(Eigen3) -string(TOLOWER "Eigen3" lc_eigen) -if(NOT ${lc_eigen}_POPULATED) - message("Checking if [Eigen3] dependency needs to be fetched...") - FetchContent_Populate(Eigen3) - set(EIGEN_INCLUDE_DIR ${${lc_eigen}_SOURCE_DIR}) - message("EIGEN_INCLUDE_DIR set to ${EIGEN_INCLUDE_DIR}") - - message("Packaging [Eigen3] headers to ${UE_PLUGIN_SIMLIBS_DIR}/eigen") - file(COPY ${EIGEN_INCLUDE_DIR}/ DESTINATION ${UE_PLUGIN_SIMLIBS_DIR}/eigen/include) +find_package(Eigen3 3.4.0 QUIET NO_MODULE) +if(Eigen3_FOUND) + get_target_property(EIGEN_INCLUDE_DIR Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES) + message("Using system [Eigen3] dependency") +else() + if(EXISTS "/usr/include/eigen3/Eigen/Core") + set(EIGEN_INCLUDE_DIR "/usr/include/eigen3") + elseif(EXISTS "/usr/local/include/eigen3/Eigen/Core") + set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") + endif() + if(EIGEN_INCLUDE_DIR) + message("Using local [Eigen3] headers") + else() + FetchContent_Declare( + Eigen3 + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG 3.4.0 + ) + FetchContent_GetProperties(Eigen3) + string(TOLOWER "Eigen3" lc_eigen) + if(NOT ${lc_eigen}_POPULATED) + message("Checking if [Eigen3] dependency needs to be fetched...") + FetchContent_Populate(Eigen3) + set(EIGEN_INCLUDE_DIR ${${lc_eigen}_SOURCE_DIR}) + endif() + endif() endif() +message("EIGEN_INCLUDE_DIR set to ${EIGEN_INCLUDE_DIR}") + +message("Packaging [Eigen3] headers to ${UE_PLUGIN_SIMLIBS_DIR}/eigen") +file(COPY ${EIGEN_INCLUDE_DIR}/ DESTINATION ${UE_PLUGIN_SIMLIBS_DIR}/eigen/include) # Set up dependency: Google Test FetchContent_Declare( @@ -323,19 +338,54 @@ if(UNIX) # so to force /MD non-debug CRT to match UE, build Debug config as Relwithdebinfo and # build Release as Release. set(OPENSSL_BUILD_TYPE $,debug,release>) + + # Build OpenSSL with the same compiler/sysroot as the rest of the project so that all + # glibc symbol version requirements are consistent. Use no-shared so only the static + # libraries (libcrypto.a / libssl.a) are produced, avoiding runtime dynamic-library + # glibc ABI mismatches when linking against the UE toolchain (CentOS 7 / glibc 2.17). + if(DEFINED ENV{UE_ROOT}) + # UE cross-compiler targets a CentOS 7 sysroot (glibc 2.17). Tell OpenSSL to use + # the same clang and sysroot so the compiled objects only reference old glibc symbols. + set(OPENSSL_CONFIGURE_COMMAND + ${CMAKE_COMMAND} -E env + CC=${CMAKE_C_COMPILER} + "CFLAGS=--sysroot=${CMAKE_SYSROOT}" + /Configure linux-x86_64 + --${OPENSSL_BUILD_TYPE} + --prefix=${CMAKE_BINARY_DIR}/_deps/openssl-install + no-tests no-shared no-module + ) + else() + # System toolchain: still force static-only build for consistency. + set(OPENSSL_CONFIGURE_COMMAND + ${CMAKE_COMMAND} -E env + CC=${CMAKE_C_COMPILER} + /Configure + --${OPENSSL_BUILD_TYPE} + --prefix=${CMAKE_BINARY_DIR}/_deps/openssl-install + no-tests no-shared no-module + ) + endif() + + set(OPENSSL_LIB_DIR ${CMAKE_BINARY_DIR}/_deps/openssl-install/lib64) + set(OPENSSL_CRYPTO_LIBRARY ${OPENSSL_LIB_DIR}/libcrypto.a) + set(OPENSSL_SSL_LIBRARY ${OPENSSL_LIB_DIR}/libssl.a) + set(OPENSSL_INCLUDE_DIR ${CMAKE_BINARY_DIR}/_deps/openssl-install/include) + ExternalProject_Add(openssl-external GIT_REPOSITORY https://github.com/openssl/openssl.git GIT_TAG openssl-3.1.1 GIT_CONFIG "advice.detachedHead=false" PREFIX ${CMAKE_BINARY_DIR}/_deps/openssl UPDATE_COMMAND "" # disable update step - CONFIGURE_COMMAND /Configure --${OPENSSL_BUILD_TYPE} --prefix=${CMAKE_BINARY_DIR}/_deps/openssl-install no-tests - BUILD_COMMAND make - INSTALL_COMMAND make install + CONFIGURE_COMMAND ${OPENSSL_CONFIGURE_COMMAND} + # Build/install only static runtime + dev artifacts; avoid build_sw/install_sw + # which attempt to link provider DSOs (e.g., providers/legacy.so). + BUILD_COMMAND make build_libs + INSTALL_COMMAND make install_dev + INSTALL_BYPRODUCTS ${OPENSSL_CRYPTO_LIBRARY} ${OPENSSL_SSL_LIBRARY} TEST_COMMAND "" # disable test step ) - set(OPENSSL_LIB_DIR ${CMAKE_BINARY_DIR}/_deps/openssl-install/lib64) - set(OPENSSL_INCLUDE_DIR ${CMAKE_BINARY_DIR}/_deps/openssl-install/include) message("OPENSSL_LIB_DIR set to ${OPENSSL_LIB_DIR}") message("OPENSSL_INCLUDE_DIR set to ${OPENSSL_INCLUDE_DIR}") ExternalProject_Add_Step(openssl-external post-install diff --git a/build.cmd b/build.cmd index 3547a7c3..e896f918 100644 --- a/build.cmd +++ b/build.cmd @@ -1,30 +1,22 @@ +@echo off REM Copyright (C) Microsoft Corporation. REM Copyright (C) 2025 IAMAI CONSULTING CORP REM MIT License. -@echo off setlocal ENABLEDELAYEDEXPANSION -set ROOT_DIR=%~dp0 +set "ROOT_DIR=%~dp0" REM ===================================================== REM Check Visual Studio environment REM ===================================================== -if "%VisualStudioVersion%"=="16.0" goto ver_ok -if "%VisualStudioVersion%"=="17.0" goto ver_ok - -echo: -echo You need to run this command from x64 Native Tools Command Prompt for VS 2019 or VS 2022. -goto :buildfailed_nomsg - -:ver_ok - -where /q nmake -if errorlevel 1 ( - echo: - echo nmake not found. - goto :buildfailed_nomsg +if "%VisualStudioVersion%"=="16.0" ( + echo Detected Visual Studio environment: VS 2019 +) else if "%VisualStudioVersion%"=="17.0" ( + echo Detected Visual Studio environment: VS 2022 +) else ( + echo Visual Studio developer environment not detected. Initializing VS 2022 build tools. ) REM ===================================================== @@ -32,8 +24,8 @@ REM Unreal Engine detection (OPTIONAL) REM UE_ROOT is optional to allow standalone / Unity builds REM ===================================================== -set UE_DETECTED=0 -set UE_MINOR= +set "UE_DETECTED=0" +set "UE_MINOR=" if "%UE_ROOT%"=="" ( echo: @@ -41,33 +33,33 @@ if "%UE_ROOT%"=="" ( goto :select_msvc_default ) -set BUILD_VERSION_FILE=%UE_ROOT%\Engine\Build\Build.version +set "BUILD_VERSION_FILE=%UE_ROOT%\Engine\Build\Build.version" -if not exist "%BUILD_VERSION_FILE%" ( +if not exist "!BUILD_VERSION_FILE!" ( echo: echo UE_ROOT is set but Build.version not found: - echo %BUILD_VERSION_FILE% + echo !BUILD_VERSION_FILE! echo Falling back to standalone build. goto :select_msvc_default ) -for /f "tokens=2 delims=:," %%A in ('findstr /i "MinorVersion" "%BUILD_VERSION_FILE%"') do ( - set UE_MINOR=%%A +for /f "tokens=2 delims=:," %%A in ('findstr /i "MinorVersion" "!BUILD_VERSION_FILE!"') do ( + set "UE_MINOR=%%A" ) -set UE_MINOR=%UE_MINOR: =% +set "UE_MINOR=%UE_MINOR: =%" echo Detected Unreal Engine version: 5.%UE_MINOR% -set UE_DETECTED=1 +set "UE_DETECTED=1" REM ===================================================== REM Select MSVC version (UE-aware) REM ===================================================== if "%UE_MINOR%"=="2" ( - set MSVC_VER=14.37 + set "MSVC_VER=14.37" ) else if "%UE_MINOR%"=="7" ( - set MSVC_VER=14.39 + set "MSVC_VER=14.39" ) else ( echo: echo Unsupported Unreal Engine version 5.%UE_MINOR% @@ -83,7 +75,7 @@ REM ===================================================== :select_msvc_default echo Using default MSVC toolset (standalone / Unity build) -set MSVC_VER=14.37 +set "MSVC_VER=" REM ===================================================== REM Initialize MSVC environment @@ -91,12 +83,79 @@ REM ===================================================== :msvc_ready -echo Using MSVC toolset version: %MSVC_VER% +if defined MSVC_VER ( + echo Requested MSVC toolset version: %MSVC_VER% +) else ( + echo Requested MSVC toolset version: latest installed +) + +set "VSWHERE=%SystemDrive%\Progra~2\Microsoft Visual Studio\Installer\vswhere.exe" +set "VS_INSTALL_DIR=" +set "MSVC_TOOLS_VERSION=" + +if defined VSINSTALLDIR set "VS_INSTALL_DIR=%VSINSTALLDIR%" + +call :check_vs_install "%ProgramFiles%\Microsoft Visual Studio\2022\Community" +call :check_vs_install "%ProgramFiles%\Microsoft Visual Studio\2022\Professional" +call :check_vs_install "%ProgramFiles%\Microsoft Visual Studio\2022\Enterprise" +call :check_vs_install "%SystemDrive%\Progra~2\Microsoft Visual Studio\2022\BuildTools" + +if defined VS_INSTALL_DIR goto :vs_install_found +if not exist "!VSWHERE!" goto :vs_install_found +for /f "tokens=*" %%I in ('call "!VSWHERE!" -version "[17.0,18.0)" -latest -requires Microsoft.VisualStudio.Component.VC.Tools.x86.x64 -property installationPath') do set "VS_INSTALL_DIR=%%I" + +:vs_install_found + +if not defined VS_INSTALL_DIR ( + echo: + echo [ERROR] Visual Studio 2022 with C++ build tools was not found. + goto :buildfailed_nomsg +) + +if defined MSVC_VER ( + for /d %%I in ("!VS_INSTALL_DIR!\VC\Tools\MSVC\%MSVC_VER%*") do ( + if not defined MSVC_TOOLS_VERSION set "MSVC_TOOLS_VERSION=%%~nxI" + ) +) else ( + for /f "delims=" %%I in ('dir /b /ad /o-n "!VS_INSTALL_DIR!\VC\Tools\MSVC" 2^>NUL') do ( + if not defined MSVC_TOOLS_VERSION set "MSVC_TOOLS_VERSION=%%I" + ) +) + +if not defined MSVC_TOOLS_VERSION ( + echo: + if defined MSVC_VER ( + echo [ERROR] MSVC toolset %MSVC_VER% was not found under: + ) else ( + echo [ERROR] No MSVC toolset was found under: + ) + echo !VS_INSTALL_DIR!\VC\Tools\MSVC + if "%UE_DETECTED%"=="1" ( + echo Install the requested MSVC toolset or update build.cmd for your Unreal Engine version. + ) else ( + echo Install the Visual Studio C++ build tools. + ) + goto :buildfailed_nomsg +) + +echo Using Visual Studio installation: %VS_INSTALL_DIR% +echo Using MSVC tools version: %MSVC_TOOLS_VERSION% -call "C:\Program Files (x86)\Microsoft Visual Studio\2022\BuildTools\VC\Auxiliary\Build\vcvarsall.bat" x64 -vcvars_ver=%MSVC_VER% +call "%VS_INSTALL_DIR%\VC\Auxiliary\Build\vcvarsall.bat" x64 -vcvars_ver=%MSVC_TOOLS_VERSION% if errorlevel 1 ( echo: - echo [WARNING] Failed to initialize MSVC %MSVC_VER% + echo [ERROR] Failed to initialize MSVC %MSVC_TOOLS_VERSION% + goto :buildfailed_nomsg +) + +set PATH=%VS_INSTALL_DIR%\MSBuild\Current\Bin\amd64;%VS_INSTALL_DIR%\MSBuild\Current\Bin;%PATH% +set MSBUILD_TOOLSET_ARGS=/p:VCToolsVersion=%MSVC_TOOLS_VERSION% + +where /q nmake +if errorlevel 1 ( + echo: + echo [ERROR] nmake not found after initializing Visual Studio build tools. + goto :buildfailed_nomsg ) REM ===================================================== @@ -115,7 +174,12 @@ REM Error handling REM ===================================================== :buildfailed_nomsg - chdir /d %ROOT_DIR% + chdir /d "%ROOT_DIR%" echo: echo Build Failed. exit /b 1 + +:check_vs_install + if defined VS_INSTALL_DIR exit /b 0 + if exist "%~1\VC\Auxiliary\Build\vcvarsall.bat" set "VS_INSTALL_DIR=%~1" + exit /b 0 diff --git a/build_linux.mk b/build_linux.mk index 34f233b5..23f236e9 100644 --- a/build_linux.mk +++ b/build_linux.mk @@ -26,8 +26,11 @@ default: @echo " all_no_test = Build + Package everything" @echo " clean = Clean sim libs + Blocks build files" @echo - @echo " simlibs_debug = Build + Package sim libs for Debug" - @echo " simlibs_release = Build + Package sim libs for Release" + @echo " simlibs_debug = Build sim libs + C++ client for Debug" + @echo " simlibs_release = Build sim libs + C++ client for Release" + @echo " cpp_client_debug = Build C++ client only for Debug" + @echo " cpp_client_release = Build C++ client only for Release" + @echo " package_cpp_client = Build Release C++ client and create versioned package" @echo " test_simlibs_debug = Test sim libs for Debug" @echo " test_simlibs_release = Test sim libs for Release" @echo @@ -62,6 +65,14 @@ CMAKE_CMD = cmake -G "Ninja" CMAKE_DBG_BUILD_CMD = cmake --build $(CMAKE_BUILD_DIR)/Debug CMAKE_REL_BUILD_CMD = cmake --build $(CMAKE_BUILD_DIR)/Release +CPP_CLIENT_DIR = client/cpp +CPP_CLIENT_DBG_BUILD_DIR = $(CPP_CLIENT_DIR)/build_linux/Debug +CPP_CLIENT_REL_BUILD_DIR = $(CPP_CLIENT_DIR)/build_linux/Release +CPP_CLIENT_CMAKE_CMD = cmake -G "Ninja" +CPP_CLIENT_DBG_BUILD_CMD = cmake --build $(CPP_CLIENT_DBG_BUILD_DIR) -j$(shell nproc) +CPP_CLIENT_REL_BUILD_CMD = cmake --build $(CPP_CLIENT_REL_BUILD_DIR) -j$(shell nproc) +CPP_CLIENT_PACKAGE_CMD = cmake --build $(CPP_CLIENT_REL_BUILD_DIR) --target package + .PHONY: config_simlibs_debug config_simlibs_debug: @echo "=======================================================================" @@ -69,8 +80,40 @@ config_simlibs_debug: mkdir -p $(CMAKE_BUILD_DIR)/Debug $(REDIRECT_OUTPUT) cd $(CMAKE_BUILD_DIR)/Debug && $(CMAKE_CMD) -DCMAKE_BUILD_TYPE=Debug ../../.. +.PHONY: config_cpp_client_debug +config_cpp_client_debug: + @echo "=======================================================================" + @echo "Configuring the C++ client project for Linux64-Debug..." + mkdir -p $(CPP_CLIENT_DBG_BUILD_DIR) $(REDIRECT_OUTPUT) + cd $(CPP_CLIENT_DBG_BUILD_DIR) && $(CPP_CLIENT_CMAKE_CMD) -DCMAKE_BUILD_TYPE=Debug $(CURDIR)/$(CPP_CLIENT_DIR) + +.PHONY: cpp_client_debug +cpp_client_debug: config_cpp_client_debug + @echo "=======================================================================" + @echo "Building the C++ client project for Linux64-Debug..." + $(CPP_CLIENT_DBG_BUILD_CMD) + +.PHONY: config_cpp_client_release +config_cpp_client_release: + @echo "=======================================================================" + @echo "Configuring the C++ client project for Linux64-Release..." + mkdir -p $(CPP_CLIENT_REL_BUILD_DIR) $(REDIRECT_OUTPUT) + cd $(CPP_CLIENT_REL_BUILD_DIR) && $(CPP_CLIENT_CMAKE_CMD) -DCMAKE_BUILD_TYPE=Release $(CURDIR)/$(CPP_CLIENT_DIR) + +.PHONY: cpp_client_release +cpp_client_release: config_cpp_client_release + @echo "=======================================================================" + @echo "Building the C++ client project for Linux64-Release..." + $(CPP_CLIENT_REL_BUILD_CMD) + +.PHONY: package_cpp_client +package_cpp_client: cpp_client_release + @echo "=======================================================================" + @echo "Packaging the C++ client for Linux64-Release..." + $(CPP_CLIENT_PACKAGE_CMD) + .PHONY: simlibs_debug -simlibs_debug: config_simlibs_debug +simlibs_debug: config_simlibs_debug cpp_client_debug @echo "=======================================================================" @echo "Building the ProjectAirSimLibs project for Linux64-Debug..." $(CMAKE_DBG_BUILD_CMD) @@ -83,7 +126,7 @@ config_simlibs_release: cd $(CMAKE_BUILD_DIR)/Release && $(CMAKE_CMD) -DCMAKE_BUILD_TYPE=Release ../../.. .PHONY: simlibs_release -simlibs_release: config_simlibs_release +simlibs_release: config_simlibs_release cpp_client_release @echo "=======================================================================" @echo "Building the ProjectAirSimLibs project for Linux64-Release..." $(CMAKE_REL_BUILD_CMD) @@ -102,9 +145,11 @@ clean: @echo "=======================================================================" @echo "Cleaning build files..." rm -fr $(CMAKE_BUILD_DIR) $(REDIRECT_OUTPUT) + rm -fr $(CPP_CLIENT_DIR)/build_linux $(REDIRECT_OUTPUT) rm -fr physics/matlab_sfunc/_deps $(REDIRECT_OUTPUT) rm -fr physics/matlab_sfunc/message $(REDIRECT_OUTPUT) rm -fr packages/projectairsim_simlibs $(REDIRECT_OUTPUT) + rm -fr packages/projectairsim_cpp_client $(REDIRECT_OUTPUT) rm -fr unreal/Blocks/Plugins/ProjectAirSim/SimLibs $(REDIRECT_OUTPUT) @echo "Cleaning Blocks build folders..." rm -fr unreal/Blocks/Binaries $(REDIRECT_OUTPUT) diff --git a/build_windows.mk b/build_windows.mk index 3a23c10b..4e8a2c25 100644 --- a/build_windows.mk +++ b/build_windows.mk @@ -11,7 +11,7 @@ # # Abstract: # -# Main makefile for Windows builds to drive CMake/UBT commands. +# Main makefile for Windows builds to drive CMake/MSBuild/UBT commands. # # --------------------------------------------------------------------------------------------------------------------- @@ -24,10 +24,13 @@ default: @echo. all = Build + Test + Package everything @echo. rebuild_all = Clean + Build + Test + Package everything @echo. all_no_test = Build + Package everything - @echo. clean = Clean sim libs + Blocks build files + @echo. clean = Clean sim libs, C++ client, and Blocks build files @echo. @echo. simlibs_debug = Build + Package sim libs for Debug @echo. simlibs_release = Build + Package sim libs for Release + @echo. cpp_client_debug = Build C++ client artifacts for Debug + @echo. cpp_client_release = Build C++ client artifacts for Release + @echo. package_cpp_client = Build Release C++ client and create versioned package @echo. test_simlibs_debug = Test sim libs for Debug @echo. test_simlibs_release = Test sim libs for Release @echo. @@ -63,6 +66,49 @@ CMAKE_CMD = cmake -G "Ninja" \ CMAKE_DBG_BUILD_CMD = cmake --build $(CMAKE_BUILD_DIR)\Debug CMAKE_REL_BUILD_CMD = cmake --build $(CMAKE_BUILD_DIR)\Release +CPP_CLIENT_DIR = client\cpp +CPP_CLIENT_BUILD_DIR = $(CPP_CLIENT_DIR)\build_windows +CPP_CLIENT_DBG_BUILD_DIR = $(CPP_CLIENT_BUILD_DIR)\Debug +CPP_CLIENT_REL_BUILD_DIR = $(CPP_CLIENT_BUILD_DIR)\Release +CPP_CLIENT_CMAKE_CMD = cmake -G "Ninja" \ + -DCMAKE_C_COMPILER=cl.exe \ + -DCMAKE_CXX_COMPILER=cl.exe +CPP_CLIENT_DBG_BUILD_CMD = cmake --build $(CPP_CLIENT_DBG_BUILD_DIR) +CPP_CLIENT_REL_BUILD_CMD = cmake --build $(CPP_CLIENT_REL_BUILD_DIR) +CPP_CLIENT_PACKAGE_CMD = cmake --build $(CPP_CLIENT_REL_BUILD_DIR) --target package + +.PHONY: config_cpp_client_debug +config_cpp_client_debug: + @echo ======================================================================= + @echo Configuring the C++ client project for Win64-Debug... + -mkdir $(CPP_CLIENT_DBG_BUILD_DIR) $(REDIRECT_OUTPUT) + cd $(CPP_CLIENT_DBG_BUILD_DIR) && $(CPP_CLIENT_CMAKE_CMD) -DCMAKE_BUILD_TYPE=Debug "%CD%\$(CPP_CLIENT_DIR)" + +.PHONY: cpp_client_debug +cpp_client_debug: config_cpp_client_debug + @echo ======================================================================= + @echo Building the C++ client artifacts for Win64-Debug... + $(CPP_CLIENT_DBG_BUILD_CMD) + +.PHONY: config_cpp_client_release +config_cpp_client_release: + @echo ======================================================================= + @echo Configuring the C++ client project for Win64-Release... + -mkdir $(CPP_CLIENT_REL_BUILD_DIR) $(REDIRECT_OUTPUT) + cd $(CPP_CLIENT_REL_BUILD_DIR) && $(CPP_CLIENT_CMAKE_CMD) -DCMAKE_BUILD_TYPE=Release "%CD%\$(CPP_CLIENT_DIR)" + +.PHONY: cpp_client_release +cpp_client_release: config_cpp_client_release + @echo ======================================================================= + @echo Building the C++ client artifacts for Win64-Release... + $(CPP_CLIENT_REL_BUILD_CMD) + +.PHONY: package_cpp_client +package_cpp_client: cpp_client_release + @echo ======================================================================= + @echo Packaging the C++ client for Win64-Release... + $(CPP_CLIENT_PACKAGE_CMD) + .PHONY: config_simlibs_debug config_simlibs_debug: @echo ======================================================================= @@ -70,12 +116,15 @@ config_simlibs_debug: -mkdir $(CMAKE_BUILD_DIR)\Debug $(REDIRECT_OUTPUT) cd $(CMAKE_BUILD_DIR)\Debug && $(CMAKE_CMD) -DCMAKE_BUILD_TYPE=Debug ..\..\.. -.PHONY: simlibs_debug -simlibs_debug: config_simlibs_debug +.PHONY: build_simlibs_debug +build_simlibs_debug: config_simlibs_debug @echo ======================================================================= @echo Building the ProjectAirSimLibs project for Win64-Debug... $(CMAKE_DBG_BUILD_CMD) +.PHONY: simlibs_debug +simlibs_debug: build_simlibs_debug cpp_client_debug + .PHONY: config_simlibs_release config_simlibs_release: @echo ======================================================================= @@ -83,12 +132,15 @@ config_simlibs_release: -mkdir $(CMAKE_BUILD_DIR)\Release $(REDIRECT_OUTPUT) cd $(CMAKE_BUILD_DIR)\Release && $(CMAKE_CMD) -DCMAKE_BUILD_TYPE=Release ..\..\.. -.PHONY: simlibs_release -simlibs_release: config_simlibs_release +.PHONY: build_simlibs_release +build_simlibs_release: config_simlibs_release @echo ======================================================================= @echo Building the ProjectAirSimLibs project for Win64-Release... $(CMAKE_REL_BUILD_CMD) +.PHONY: simlibs_release +simlibs_release: build_simlibs_release cpp_client_release + .PHONY: package_simlibs package_simlibs: simlibs_debug simlibs_release @echo ======================================================================= @@ -103,9 +155,14 @@ clean: @echo ======================================================================= @echo Cleaning build files... -rmdir /S /Q $(CMAKE_BUILD_DIR) $(REDIRECT_OUTPUT) + -rmdir /S /Q $(CPP_CLIENT_BUILD_DIR) $(REDIRECT_OUTPUT) -rmdir /S /Q physics\matlab_sfunc\_deps $(REDIRECT_OUTPUT) -rmdir /S /Q physics\matlab_sfunc\message $(REDIRECT_OUTPUT) + -rmdir /S /Q client\cpp\libraries $(REDIRECT_OUTPUT) + -rmdir /S /Q client\cpp\example_user_apps\HelloDrone\x64 $(REDIRECT_OUTPUT) + -rmdir /S /Q client\cpp\example_user_apps\HelloDrone\HelloDrone $(REDIRECT_OUTPUT) -rmdir /S /Q packages\projectairsim_simlibs $(REDIRECT_OUTPUT) + -rmdir /S /Q packages\projectairsim_cpp_client $(REDIRECT_OUTPUT) -rmdir /S /Q unreal\Blocks\Plugins\ProjectAirSim\SimLibs $(REDIRECT_OUTPUT) @echo Cleaning Blocks build folders... -rmdir /S /Q unreal\Blocks\Binaries $(REDIRECT_OUTPUT) diff --git a/client/cpp/AirSimClientDLL/AirSimClient.rc b/client/cpp/AirSimClientDLL/AirSimClient.rc deleted file mode 100644 index b82a3501f0803b74e29b2fe3329e545b34efc2c8..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4724 zcmdUzTW=CU6vxlAiQi$RFE$!VFNyK71&TyUn{tUJO-NfVVh|djQ4>GA`uopx$}TJz zXdf_}WoFNw`)$th=l7oNS!8F{w_6+A$R^xfn*wVBo7lZw+kmIMz2SG0PS#%93#9D1bqDB>w*#=-4mYzvpS3TRaoD2Vad=%vC*H?) zVKu8;!wOcmb8Fe96@eDmSFK{7fzdGUzMRrxV#P=6hw3MLEc?PIHnppA?sfGWp7o=Bt$O)~w_wiXeX5`+VV0+1R3SmbNS9<5Y zvfM-WRs6iiM5Vs6iHx+4pRvV$ld&uM))g7;d;d(!Wf~oBsL{G|c3eMhQ)%ik{!))f z>o<$p7GktrT_vuKG}Logb@uCopJ4Hbcm#N`q)GL)z$sGaHFpMAd6wm;)S0N`ZvtG@ z5g29ax;TfUQ!B@PNcHb?AJ{v7b53%^d4CDt0#%Pd&bFg*ngz&(3vOrZ8d%_hOcu=Qj-+t=;wJKp`)&& z2b!7p>dZG6Xz1|{7T){*nZyc`(|dus?K%FHXEJCoCH|V!dPJ;`WgX%+MAy+=b*6I* zRg&Z?OLF$!S#-m1vd(8TX}v1E4QrD)YUYs-GU%<@=#JBGXg|rrYo0p5b(UmOr(02s zq0q0+OrPoL8l}q&ekmX+KJNxj%Vl36b7WtkyG63PNFPXzU(I)6($ zj>qHgV@m}&Jtsx+ zyTcR8(jL>$^fB6tZ!{_8-Q*IiT}S5mgA7%J>GFZnavO(dln(dF44rV<3}%V(sh!(p$<3og$c2XSGwv`XMpcXdfIoVP&jqG zzB6rSJAYoNGSg;)=b6&u=ks>$WqthiC+Dhfu*I2ZyDW-pBg^z0)~9}rYerrDH6DB! Z*J8W&(s20w^(_i?PHe>(* diff --git a/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.sln b/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.sln deleted file mode 100644 index 39b8db10..00000000 --- a/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.sln +++ /dev/null @@ -1,65 +0,0 @@ - -Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio Version 17 -VisualStudioVersion = 17.4.33213.308 -MinimumVisualStudioVersion = 10.0.40219.1 -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AirSimClient", "AirSimClientDLL_2019.vcxproj", "{25C9B907-266E-45D2-937A-2146F964FD46}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AirSimMessage", "..\AirSimMessageLib\AirSimMessage_2019.vcxproj", "{954F33C4-81F0-4EB6-8380-7D7406CB9606}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "NNGI", "..\NNGI\NNGI_2019.vcxproj", "{7634B5D1-7E21-4177-ADBD-6496CF1C123F}" -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|x64 = Debug|x64 - Debug|x86 = Debug|x86 - Release w Symbols|x64 = Release w Symbols|x64 - Release w Symbols|x86 = Release w Symbols|x86 - Release|x64 = Release|x64 - Release|x86 = Release|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {25C9B907-266E-45D2-937A-2146F964FD46}.Debug|x64.ActiveCfg = Debug|x64 - {25C9B907-266E-45D2-937A-2146F964FD46}.Debug|x64.Build.0 = Debug|x64 - {25C9B907-266E-45D2-937A-2146F964FD46}.Debug|x86.ActiveCfg = Debug|Win32 - {25C9B907-266E-45D2-937A-2146F964FD46}.Debug|x86.Build.0 = Debug|Win32 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release w Symbols|x64.ActiveCfg = Release w Symbols|x64 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release w Symbols|x64.Build.0 = Release w Symbols|x64 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release w Symbols|x86.ActiveCfg = Release w Symbols|Win32 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release w Symbols|x86.Build.0 = Release w Symbols|Win32 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release|x64.ActiveCfg = Release|x64 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release|x64.Build.0 = Release|x64 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release|x86.ActiveCfg = Release|Win32 - {25C9B907-266E-45D2-937A-2146F964FD46}.Release|x86.Build.0 = Release|Win32 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Debug|x64.ActiveCfg = Debug|x64 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Debug|x64.Build.0 = Debug|x64 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Debug|x86.ActiveCfg = Debug|Win32 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Debug|x86.Build.0 = Debug|Win32 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release w Symbols|x64.ActiveCfg = Release w Symbols|x64 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release w Symbols|x64.Build.0 = Release w Symbols|x64 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release w Symbols|x86.ActiveCfg = Release w Symbols|Win32 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release w Symbols|x86.Build.0 = Release w Symbols|Win32 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release|x64.ActiveCfg = Release|x64 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release|x64.Build.0 = Release|x64 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release|x86.ActiveCfg = Release|Win32 - {954F33C4-81F0-4EB6-8380-7D7406CB9606}.Release|x86.Build.0 = Release|Win32 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Debug|x64.ActiveCfg = Debug|x64 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Debug|x64.Build.0 = Debug|x64 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Debug|x86.ActiveCfg = Debug|Win32 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Debug|x86.Build.0 = Debug|Win32 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release w Symbols|x64.ActiveCfg = Release w Symbols|x64 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release w Symbols|x64.Build.0 = Release w Symbols|x64 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release w Symbols|x86.ActiveCfg = Release w Symbols|Win32 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release w Symbols|x86.Build.0 = Release w Symbols|Win32 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release|x64.ActiveCfg = Release|x64 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release|x64.Build.0 = Release|x64 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release|x86.ActiveCfg = Release|Win32 - {7634B5D1-7E21-4177-ADBD-6496CF1C123F}.Release|x86.Build.0 = Release|Win32 - EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection - GlobalSection(ExtensibilityGlobals) = postSolution - SolutionGuid = {75BBF75C-BF60-4715-8D31-E488A3356B7C} - EndGlobalSection -EndGlobal diff --git a/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj b/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj deleted file mode 100644 index e0f89014..00000000 --- a/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj +++ /dev/null @@ -1,331 +0,0 @@ - - - - - Debug - Win32 - - - Release w Symbols - Win32 - - - Release w Symbols - x64 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 16.0 - Win32Proj - {25c9b907-266e-45d2-937a-2146f964fd46} - AirSimClientDLL - 10.0 - AirSimClient.2019 - - - - DynamicLibrary - true - v142 - Unicode - - - DynamicLibrary - false - v142 - true - Unicode - - - DynamicLibrary - false - v142 - true - Unicode - - - DynamicLibrary - true - v142 - Unicode - - - DynamicLibrary - false - v142 - true - Unicode - - - DynamicLibrary - false - v142 - true - Unicode - - - - - - - - - - - - - - - - - - - - - - - - - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level4 - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;_DEBUG;WIN32;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDebugDLL - stdcpp17 - OnlyExplicitInline - - - Windows - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;WIN32;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;WIN32;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - Disabled - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;_DEBUG;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDebugDLL - stdcpp17 - OnlyExplicitInline - - - Windows - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - Disabled - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Create - Create - Create - Create - Create - Create - - - - - - - - - - NotUsing - NotUsing - NotUsing - NotUsing - NotUsing - NotUsing - - - - - - - - - - - - - - - {954f33c4-81f0-4eb6-8380-7d7406cb9606} - - - {7634b5d1-7e21-4177-adbd-6496cf1c123f} - - - - - - \ No newline at end of file diff --git a/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj.filters b/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj.filters deleted file mode 100644 index cdd0cb23..00000000 --- a/client/cpp/AirSimClientDLL/AirSimClientDLL_2019.vcxproj.filters +++ /dev/null @@ -1,143 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Source Files - - - Source Files - - - Header Files - - - Source Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Source Files - - - Header Files - - - Header Files - - - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - - - Resource Files - - - \ No newline at end of file diff --git a/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.sln b/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.sln deleted file mode 100644 index 98fff6c5..00000000 --- a/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.sln +++ /dev/null @@ -1,65 +0,0 @@ - -Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio Version 17 -VisualStudioVersion = 17.4.33213.308 -MinimumVisualStudioVersion = 10.0.40219.1 -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AirSimClient.2022", "AirSimClientDLL_2022.vcxproj", "{FDEC21CD-0370-44FB-97BE-D3608F0A440B}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AirSimMessage.2022", "..\AirSimMessageLib\AirSimMessage_2022.vcxproj", "{70A957FF-AA39-4FE2-A55B-EADEC72CC665}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "NNGI.2022", "..\NNGI\NNGI_2022.vcxproj", "{782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}" -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|x64 = Debug|x64 - Debug|x86 = Debug|x86 - Release w Symbols|x64 = Release w Symbols|x64 - Release w Symbols|x86 = Release w Symbols|x86 - Release|x64 = Release|x64 - Release|x86 = Release|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Debug|x64.ActiveCfg = Debug|x64 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Debug|x64.Build.0 = Debug|x64 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Debug|x86.ActiveCfg = Debug|Win32 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Debug|x86.Build.0 = Debug|Win32 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release w Symbols|x64.ActiveCfg = Release w Symbols|x64 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release w Symbols|x64.Build.0 = Release w Symbols|x64 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release w Symbols|x86.ActiveCfg = Release w Symbols|Win32 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release w Symbols|x86.Build.0 = Release w Symbols|Win32 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release|x64.ActiveCfg = Release|x64 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release|x64.Build.0 = Release|x64 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release|x86.ActiveCfg = Release|Win32 - {FDEC21CD-0370-44FB-97BE-D3608F0A440B}.Release|x86.Build.0 = Release|Win32 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Debug|x64.ActiveCfg = Debug|x64 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Debug|x64.Build.0 = Debug|x64 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Debug|x86.ActiveCfg = Debug|Win32 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Debug|x86.Build.0 = Debug|Win32 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release w Symbols|x64.ActiveCfg = Release w Symbols|x64 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release w Symbols|x64.Build.0 = Release w Symbols|x64 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release w Symbols|x86.ActiveCfg = Release w Symbols|Win32 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release w Symbols|x86.Build.0 = Release w Symbols|Win32 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release|x64.ActiveCfg = Release|x64 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release|x64.Build.0 = Release|x64 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release|x86.ActiveCfg = Release|Win32 - {70A957FF-AA39-4FE2-A55B-EADEC72CC665}.Release|x86.Build.0 = Release|Win32 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Debug|x64.ActiveCfg = Debug|x64 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Debug|x64.Build.0 = Debug|x64 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Debug|x86.ActiveCfg = Debug|Win32 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Debug|x86.Build.0 = Debug|Win32 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release w Symbols|x64.ActiveCfg = Release w Symbols|x64 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release w Symbols|x64.Build.0 = Release w Symbols|x64 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release w Symbols|x86.ActiveCfg = Release w Symbols|Win32 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release w Symbols|x86.Build.0 = Release w Symbols|Win32 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release|x64.ActiveCfg = Release|x64 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release|x64.Build.0 = Release|x64 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release|x86.ActiveCfg = Release|Win32 - {782906B0-DCEF-4FB5-A5AB-381D8EEA7C97}.Release|x86.Build.0 = Release|Win32 - EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection - GlobalSection(ExtensibilityGlobals) = postSolution - SolutionGuid = {75BBF75C-BF60-4715-8D31-E488A3356B7C} - EndGlobalSection -EndGlobal diff --git a/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj b/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj deleted file mode 100644 index 55533cef..00000000 --- a/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj +++ /dev/null @@ -1,331 +0,0 @@ - - - - - Debug - Win32 - - - Release w Symbols - Win32 - - - Release w Symbols - x64 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 16.0 - Win32Proj - {FDEC21CD-0370-44fb-97BE-D3608F0A440B} - AirSimClientDLL - 10.0 - AirSimClient.2022 - - - - DynamicLibrary - true - v143 - Unicode - - - DynamicLibrary - false - v143 - true - Unicode - - - DynamicLibrary - false - v143 - true - Unicode - - - DynamicLibrary - true - v143 - Unicode - - - DynamicLibrary - false - v143 - true - Unicode - - - DynamicLibrary - false - v143 - true - Unicode - - - - - - - - - - - - - - - - - - - - - - - - - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\Build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level4 - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;_DEBUG;WIN32;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDebugDLL - stdcpp17 - OnlyExplicitInline - - - Windows - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;WIN32;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;WIN32;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - Disabled - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;_DEBUG;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDebugDLL - stdcpp17 - OnlyExplicitInline - - - Windows - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - Level4 - true - true - true - AIRSIMCLIENT_LIB_BUILD;NNG_STATIC_LIB;AIRSIMCLIENTDLL_EXPORTS;_WINDOWS;_USRDLL;NDEBUG;%(PreprocessorDefinitions) - true - Use - pch.h - $(ProjectDir)include\AirSimClient\;$(ProjectDir)..\AirSimMessageLib\include\;$(ProjectDir)..\NNGI\include\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;%(AdditionalIncludeDirectories) - MultiThreadedDLL - stdcpp17 - Disabled - - - Windows - true - true - true - false - Version.lib;Ws2_32.lib;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nng-install\lib\nng.lib;$(CoreLibraryDependencies);%(AdditionalDependencies) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Create - Create - Create - Create - Create - Create - - - - - - - - - - NotUsing - NotUsing - NotUsing - NotUsing - NotUsing - NotUsing - - - - - - - - - - - - - - - {70a957ff-aa39-4fe2-a55b-eadec72cc665} - - - {782906b0-dcef-4fb5-a5ab-381d8eea7c97} - - - - - - \ No newline at end of file diff --git a/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj.filters b/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj.filters deleted file mode 100644 index cdd0cb23..00000000 --- a/client/cpp/AirSimClientDLL/AirSimClientDLL_2022.vcxproj.filters +++ /dev/null @@ -1,143 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Source Files - - - Source Files - - - Header Files - - - Source Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Source Files - - - Header Files - - - Header Files - - - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - - - Resource Files - - - \ No newline at end of file diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/ASCDecl.h b/client/cpp/AirSimClientDLL/Include/AirSimClient/ASCDecl.h deleted file mode 100644 index 4e907e38..00000000 --- a/client/cpp/AirSimClientDLL/Include/AirSimClient/ASCDecl.h +++ /dev/null @@ -1,14 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -#pragma once - -#ifndef ASC_DECL -#ifdef AIRSIMCLIENT_LIB_BUILD -#define ASC_DECL __declspec(dllexport) -#else -#define ASC_DECL __declspec(dllimport) -#endif // AIRSIMCLIENT_LIB_BUILD -#endif // ASC_DECL diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/Drone.h b/client/cpp/AirSimClientDLL/Include/AirSimClient/Drone.h deleted file mode 100644 index f77cc0ee..00000000 --- a/client/cpp/AirSimClientDLL/Include/AirSimClient/Drone.h +++ /dev/null @@ -1,150 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -#pragma once -#include -#include - -#include "ASCDecl.h" -#include "Client.h" -#include "World.h" - -namespace microsoft { -namespace projectairsim { -namespace client { - -class Drone { - public: - // Camera sensor image types - enum class ImageType : int { - Scene = 0, - DepthPlanar = 1, - DepthPerspective = 2, - Segmentation = 3, - DepthVis = 4, - DisparityNormalized = 5, - SurfaceNormals = 6, - // kInfrared = 7, // this type not implemented yet - - // The following must be last - MAX - }; - - // Yaw control modes - enum class YawControlMode { - MaxDegreeOfFreedom = - 0, // Yaw angle, if specified, is relative to initial yaw - ForwardOnly = - 1, // Yaw angle, if specified, is relative to direction of travel - }; // enum class YawControlMode - - // VTOL flight modes - enum class VTOLMode { - Multirotor = 0, // Multirotor (helicopter) mode - FixedWing = 1, // Fixed-wing mode when possible, multirotor otherwise - }; // enum class VTOLMode - - public: - // Timeout value meaning no timeout - ASC_DECL static const float kNoTimeout; - - public: - ASC_DECL Drone(void) noexcept; - ASC_DECL ~Drone(); - - // Initialize the drone object. Connects to named drone in - // simulation connected to the client and in the specified world. - // - // Arguments: - // pclient Pointer to client object - // pworld Pointer to world object - // drone_name Name of the drone to which the object attaches - // - // Returns: - // (Return) Initialization status - ASC_DECL Status Initialize(std::shared_ptr& pclient, - std::shared_ptr& pworld, - const std::string& drone_name); - - // API control - ASC_DECL Status CancelLastTask(bool* pf_task_is_canceled_out); - ASC_DECL Status DisableAPIControl(bool* pf_is_disabled_out); - ASC_DECL Status EnableAPIControl(bool* pf_is_enabled_out); - ASC_DECL Status IsAPIControlEnabled(bool* pf_is_enabled_out) const; - - // Arming control - ASC_DECL Status Arm(bool* pf_is_armed_out); - ASC_DECL Status CanArm(bool* pf_can_be_armed_out) const; - ASC_DECL Status Disarm(bool* pf_is_disarmed_out); - - // Topic names - ASC_DECL size_t GetRobotInfoCount(void) const; - ASC_DECL const char* GetRobotInfoName(int iname) const; - ASC_DECL const char* GetRobotTopic(const std::string& str_info_name) const; - ASC_DECL size_t GetSensorCount(void) const; - ASC_DECL size_t GetSensorInfoCount(const std::string& str_sensor_name) const; - ASC_DECL const char* GetSensorInfoName(const std::string& str_sensor_name, - int iinfo) const; - ASC_DECL const char* GetSensorName(int iname) const; - ASC_DECL const char* GetSensorTopic(const std::string& str_sensor_name, - const std::string& str_info_name) const; - - // Sensors - ASC_DECL Status GetCameraRay(const std::string& str_camera_name, - ImageType image_type, int x, int y, - Pose* ppose_out); - - // Drone state - ASC_DECL Status GetGroundTruthPose(Transform* ptransform_out); - ASC_DECL Status SetPose(const Transform& transform, bool reset_kinematics); - - // Take-off and landing - ASC_DECL AsyncResult - LandAsync(float sec_timeout = kNoTimeout, - FnResponseCallback fnresponse_callback = nullptr); - ASC_DECL AsyncResult - TakeoffAsync(float sec_timeout = kNoTimeout, - FnResponseCallback fnresponse_callback = nullptr); - - // Movement - ASC_DECL AsyncResult - GoHomeAsync(float sec_timeout = kNoTimeout, float velocity = 0.5f, - FnResponseCallback fnresponse_callback = nullptr); - ASC_DECL AsyncResult - HoverAsync(FnResponseCallback fnresponse_callback = nullptr); - ASC_DECL AsyncResult MoveByVelocityAsync( - float v_north, float v_east, float v_down, float sec_duration, - YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, - bool yaw_is_rate = true, float yaw = 0.0f, - FnResponseCallback fnresponse_callback = nullptr); - ASC_DECL AsyncResult MoveByVelocityBodyFrameAsync( - float v_forward, float v_right, float v_down, float sec_duration, - YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, - bool yaw_is_rate = true, float yaw = 0.0f, - FnResponseCallback fnresponse_callback = nullptr); - ASC_DECL AsyncResult MoveOnPathAsync( - const VecVector3& path, float velocity, float timeout_sec = kNoTimeout, - YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, - bool yaw_is_rate = true, float yaw = 0.0f, float lookahead = -1, - float adaptive_lookahead = 1.0f, - FnResponseCallback fnresponse_callback = nullptr); - ASC_DECL AsyncResult MoveToPositionAsync( - float north, float east, float down, float velocity, - float timeout_sec = kNoTimeout, - YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, - bool yaw_is_rate = true, float yaw = 0.0, float lookahead = -1.0, - float adaptive_lookahead = 1.0, - FnResponseCallback fnresponse_callback = nullptr); - - protected: - class Impl; // Drone implementation - - protected: - std::shared_ptr pimpl_; // Pointer to implementation -}; // class Drone - -} // namespace client -} // namespace projectairsim -} // namespace microsoft diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/World.h b/client/cpp/AirSimClientDLL/Include/AirSimClient/World.h deleted file mode 100644 index 7c102251..00000000 --- a/client/cpp/AirSimClientDLL/Include/AirSimClient/World.h +++ /dev/null @@ -1,156 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -#pragma once -#include "ASCDecl.h" -#include "Client.h" -#include "Common.h" -#include "Status.h" -#include "Types.h" - -namespace microsoft { -namespace projectairsim { -namespace client { - -class World { - public: - ASC_DECL World(void) noexcept; - ASC_DECL ~World(); - - // Initializes the world. If a scene config file name is given, the - // scene is loaded into the simulation. If a scene config file name is - // not given, this object reflects scene currently loaded into the - // simulator. (Note that the scene configuration JSON will not be - // available when using the currently loaded scene.) - // - // Arguments: - // pclient Pointer to client object - // scene_config_name Path to scene config file - // sim_config_path Path to directory containing files referenced by - // the scene config file delay_after_loading_sec Time period to sleep after - // loading the scene (seconds) sim_instance_idx Simulation instance - // number to append to scene ID (used for parallel cooperating simulations) - // - // Returns: - // (Return) Initialization status - ASC_DECL Status Initialize(std::shared_ptr& pclient, - const std::string& scene_config_name = "", - const std::string& sim_config_path = "sim_config/", - float delay_after_load_sec = 0, - int sim_instance_idx = -1); - - // Returns a voxel occupation grid for the specified volume. The returned - // boolean array is the 3D voxel occupancy map returned as a linear array. - // A true value indicates the voxel is occupied while false means the voxel - // is empty. The number of elements in the array is (nx * ny * nz) where: - // nx = floor(x_size / resolution) - // ny = floor(y_size / resolution) - // nz = floor(z_size / resolution) - // - // The entries in the array are for voxels in the order: - // (x0, y0, z0), (x0, y0, z1), ..., (x0, y0, znz-1), - // (x0, y1, z0), (x0, y1, z1), ..., (x0, y1, znz-1), - // ... - // (x0, yny-1, z0), (x0, yny-1, z1), ..., (x0, yny-1, znz-1), - // (x1, y0, z0), (x1, y0, z1), ..., (x1, y0, znz-1), - // ... - // (xnx-1, yny-1, z0), (xnx-1, yny-1, z1),..., (xnx-1, yny-1, znz-1) - // - // where (x0, y0, and z0) is the voxel at the minimum north, east, down - // position (far back, left, and up) while (xnx-1, ynx-1, znz-1) is at the - // maximum north, east, down position (far front, right, and down.) Thus, - // the voxel at position (i, j, k) is at index (i * ny * nz + j * nz + k) - // of the returned boolean array. - // - // - // Arguments: - // position Center of volume to inspect (orientation ignored) - // x_size Volume length along X-axis (meters) - // y_size Volume length along Y-axis (meters) - // z_size Volume length along Z-axis (meters) - // resolution Length of each edge of a voxel (meters) - // pbool_array_ret The buffer to get the boolean area indicating occupancy - // actors_to_ignore IDs of actors to ignore for occupancy - // write_file If true, write the voxel grid to file_path in binvox - // format file_path Path to output file if write_file is true - // - // Returns: - // (Return) Call status - // pbool_array_ret If call status is Status::OK, updated with the occupancy - // array - ASC_DECL Status - CreateVoxelGrid(const struct Pose& position, int x_size, int y_size, - int z_size, float resolution, BoolArray* pbool_array_ret, - const std::vector& actors_to_ignore = - std::vector(), - bool write_file = false, - std::string file_path = "./voxel_grid.binvox") const; - - // Return the scene configuration JSON loaded for this world (empty if - // Initialize() was not called with a scene config file name.) - ASC_DECL const json& GetConfiguration(void) const; - - // Return the names of the drones in the scene - ASC_DECL const VecStr& GetDrones(void) const; - - // Return the prefix for topics associated with this scene - ASC_DECL const char* GetParentTopic(void) const; - - // - // Debug Plots - // - ASC_DECL Status FlushPersistentMarkers(void); - ASC_DECL Status PlotDebugArrows(const VecVector3& points_start, - const VecVector3& points_end, - const ColorRGBA& color_rgba, float thickness, - float arrow_size, float sec_duration, - bool is_persistent); - ASC_DECL Status PlotDebugDashedLine(const VecVector3& points, - const ColorRGBA& color_rgba, - float thickness, float sec_duration, - bool is_persistent); - ASC_DECL Status PlotDebugPoints(const VecVector3& points, - const ColorRGBA& color_rgba, float size, - float sec_duration, bool is_persistent); - ASC_DECL Status PlotDebugSolidLine(const VecVector3& points, - const ColorRGBA& color_rgba, - float thickness, float sec_duration, - bool is_persistent); - ASC_DECL Status PlotDebugStrings(const VecStr& strings, - const VecVector3& positions, float scale, - const ColorRGBA& color_rgba, - float sec_duration); - ASC_DECL Status PlotDebugTransforms(const VecPose& poses, float scale, - float thickness, float sec_duration, - bool is_persistent); - - // - // Interactive UI Methods - // - - // Return the 3D position of the first location hit by a ray projected - // along the local X-axis of the specified pose. - // - // Arguments: - // pose Ray is projected from this point at this orientation - // pvec3_out Buffer to get hit location - // - // Returns: - // (Return) Operation status - // pvec3_out If return status is Status::OK, contains the location - // where the ray first hit an object, or (NaN, NaN, Nan) - // if nothing was hit (NED coordinates) - ASC_DECL Status HitTest(const Pose& pose, Vector3* pvec3_out); - - protected: - class Impl; // World implementation - - protected: - std::shared_ptr pimpl_; // Pointer to implementation -}; // class World - -} // namespace client -} // namespace projectairsim -} // namespace microsoft diff --git a/client/cpp/AirSimClientDLL/dllmain.cpp b/client/cpp/AirSimClientDLL/dllmain.cpp deleted file mode 100644 index f9deb441..00000000 --- a/client/cpp/AirSimClientDLL/dllmain.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -// dllmain.cpp : Defines the entry point for the DLL application. -#include "pch.h" -#include "src/Utils.h" - -BOOL APIENTRY DllMain(HMODULE hModule, DWORD ul_reason_for_call, - LPVOID /*lpReserved*/ -) { - switch (ul_reason_for_call) { - case DLL_PROCESS_ATTACH: - microsoft::projectairsim::client::internal::UpdateClientVersion(hModule); - break; - - case DLL_THREAD_ATTACH: - case DLL_THREAD_DETACH: - case DLL_PROCESS_DETACH: - break; - } - return TRUE; -} diff --git a/client/cpp/AirSimClientDLL/framework.h b/client/cpp/AirSimClientDLL/framework.h deleted file mode 100644 index 5ca6d6fb..00000000 --- a/client/cpp/AirSimClientDLL/framework.h +++ /dev/null @@ -1,10 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -#pragma once - -#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers -// Windows Header Files -#include diff --git a/client/cpp/AirSimClientDLL/pch.cpp b/client/cpp/AirSimClientDLL/pch.cpp deleted file mode 100644 index a3a52423..00000000 --- a/client/cpp/AirSimClientDLL/pch.cpp +++ /dev/null @@ -1,10 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -// pch.cpp: source file corresponding to the pre-compiled header - -#include "pch.h" - -// When you are using pre-compiled headers, this source file is necessary for compilation to succeed. diff --git a/client/cpp/AirSimClientDLL/resource.h b/client/cpp/AirSimClientDLL/resource.h deleted file mode 100644 index d4b94cef..00000000 --- a/client/cpp/AirSimClientDLL/resource.h +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -//{{NO_DEPENDENCIES}} -// Microsoft Visual C++ generated include file. -// Used by AirSimClient.rc - -// Next default values for new objects -// -#ifdef APSTUDIO_INVOKED -#ifndef APSTUDIO_READONLY_SYMBOLS -#define _APS_NEXT_RESOURCE_VALUE 101 -#define _APS_NEXT_COMMAND_VALUE 40001 -#define _APS_NEXT_CONTROL_VALUE 1001 -#define _APS_NEXT_SYMED_VALUE 101 -#endif -#endif diff --git a/client/cpp/AirSimClientDLL/src/Drone.cpp b/client/cpp/AirSimClientDLL/src/Drone.cpp deleted file mode 100644 index cbcc3571..00000000 --- a/client/cpp/AirSimClientDLL/src/Drone.cpp +++ /dev/null @@ -1,892 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -#include "Drone.h" - -#include -#include -#include -#include - -#include "AirSimClient.h" -#include "AirSimMessage/response_message.hpp" -#include "AsyncResultInternal.h" -#include "Common.h" -#include "JSONUtils.h" -#include "pch.h" - -namespace microsoft { -namespace projectairsim { -namespace client { - -class Drone::Impl { - public: - Impl(void) noexcept; - ~Impl(); - - Status Arm(bool* pf_is_armed_out); - Status CanArm(bool* pf_can_be_armed_out) const; - Status CancelLastTask(bool* pf_task_is_canceled_out); - Status DisableAPIControl(bool* pf_is_disabled_out); - Status Disarm(bool* pf_is_disarmed_out); - Status EnableAPIControl(bool* pf_is_enabled_out); - Status GetCameraRay(const std::string& str_camera_name, ImageType image_type, - int x, int y, Pose* ppose_out); - Status GetGroundTruthPose(Transform* ptransform_out); - size_t GetRobotInfoCount(void) const; - const char* GetRobotInfoName(int iname) const; - const char* GetRobotTopic(const std::string& str_info_name) const; - size_t GetSensorCount(void) const; - size_t GetSensorInfoCount(const std::string& str_sensor_name) const; - const char* GetSensorInfoName(const std::string& str_sensor_name, - int iinfo) const; - const char* GetSensorName(int iname) const; - const char* GetSensorTopic(const std::string& str_sensor_name, - const std::string& str_info_name) const; - AsyncResult GoHomeAsync(float sec_timeout, float velocity, - FnResponseCallback fnresponse_callback); - AsyncResult HoverAsync(FnResponseCallback fnresponse_callback); - Status Initialize(std::shared_ptr& pclient, - std::shared_ptr& pworld, - const std::string& str_drone_name); - Status IsAPIControlEnabled(bool* pf_is_enabled_out) const; - AsyncResult LandAsync(float sec_timeout, - FnResponseCallback fnresponse_callback); - AsyncResult MoveByVelocityAsync(float v_north, float v_east, float v_down, - float sec_duration, - YawControlMode yaw_control_mode, - bool yaw_is_rate, float yaw, - FnResponseCallback fnresponse_callback); - ASC_DECL AsyncResult MoveByVelocityBodyFrameAsync( - float v_forward, float v_right, float v_down, float sec_duration, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - FnResponseCallback fnresponse_callback); - ASC_DECL AsyncResult MoveOnPathAsync(const VecVector3& path, float velocity, - float timeout_sec, - YawControlMode yaw_control_mode, - bool yaw_is_rate, float yaw, - float lookahead, - float adaptive_lookahead, - FnResponseCallback fnresponse_callback); - ASC_DECL AsyncResult MoveToPositionAsync( - float north, float east, float down, float velocity, float timeout_sec, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - float lookahead, float adaptive_lookahead, - FnResponseCallback fnresponse_callback); - Status SetPose(const Transform& transform, bool reset_kinematics); - AsyncResult TakeoffAsync(float sec_timeout, - FnResponseCallback fnresponse_callback); - - protected: - // Mapping from a string to another - typedef std::unordered_map UMStrStr; - - // Mapping from a string to a string-to-string map - typedef std::unordered_map UMStrUMStr; - - // Mapping from a string to vector of strings - typedef std::unordered_map> UMStrVecStr; - - protected: - void LogTopics(void) const; - void SetRobotInfoTopics(void); - void SetSensorTopics(World* pworld); - void SetTopics(void); - Status RequestDrone(const std::string str_method, - const json& json_params = json()) const; - Status RequestDrone(const std::string str_method, bool* pf_out) const; - Status RequestDrone(const std::string str_method, json* pjson_out) const; - Status RequestDrone(const std::string str_method, const json& json_params, - bool* pf_out) const; - Status RequestDrone(const std::string str_method, const json& json_params, - json* pjson_out) const; - AsyncResult RequestDroneAsync(const std::string str_method, - const json& json_params, - FnResponseCallback fnresponse_callback) const; - - protected: - std::shared_ptr pclient_; // Pointer to client connection object - std::shared_ptr pworld_; // Pointer to scene object - std::string str_drone_name_; // Name of the drone - std::string str_parent_topic_; // Topic root of the drone - std::string str_sensor_topic_; // Topic root of the sensors - std::string str_world_parent_topic_; // Topic root of the scene - UMStrStr umrobot_name_topic_; // Mapping from robot info name to topic name - UMStrUMStr umsensor_name_uminfo_topic_; // Mapping from sensor name to - // mapping from info to topic name - UMStrVecStr umsensor_name_vecinfo_; // Mapping from sensor name to list of - // sensor info names - VecStr vecstr_robot_info_name_; // List of robot info names - VecStr vecstr_sensor_name_; // List of sensor names -}; // class Drone::Impl - -ASC_DECL const float Drone::kNoTimeout = std::numeric_limits::max(); - -ASC_DECL Drone::Drone(void) noexcept : pimpl_(new Impl()) {} - -ASC_DECL Drone::~Drone() {} - -Status Drone::Initialize(std::shared_ptr& pclient, - std::shared_ptr& pworld, - const std::string& str_drone_name) { - RETURN_CATCH_STATUS(pimpl_->Initialize(pclient, pworld, str_drone_name)); -} - -ASC_DECL Status Drone::Arm(bool* pf_is_armed_out) { - RETURN_CATCH_STATUS(pimpl_->Arm(pf_is_armed_out)); -} - -ASC_DECL Status Drone::Disarm(bool* pf_is_disarmed_out) { - RETURN_CATCH_STATUS(pimpl_->Disarm(pf_is_disarmed_out)); -} - -ASC_DECL Status Drone::CanArm(bool* pf_can_be_armed_out) const { - RETURN_CATCH_STATUS(pimpl_->CanArm(pf_can_be_armed_out)); -} - -ASC_DECL Status Drone::CancelLastTask(bool* pf_task_is_canceled_out) { - RETURN_CATCH_STATUS(pimpl_->CancelLastTask(pf_task_is_canceled_out)); -} - -ASC_DECL Status Drone::DisableAPIControl(bool* pf_is_disabled_out) { - RETURN_CATCH_STATUS(pimpl_->DisableAPIControl(pf_is_disabled_out)); -} - -ASC_DECL Status Drone::EnableAPIControl(bool* pf_is_enabled_out) { - RETURN_CATCH_STATUS(pimpl_->EnableAPIControl(pf_is_enabled_out)); -} - -ASC_DECL Status Drone::GetCameraRay(const std::string& str_camera_name, - ImageType image_type, int x, int y, - Pose* ppose_out) { - RETURN_CATCH_STATUS( - pimpl_->GetCameraRay(str_camera_name, image_type, x, y, ppose_out)); -} - -ASC_DECL Status Drone::GetGroundTruthPose(Transform* ptransform_out) { - RETURN_CATCH_STATUS(pimpl_->GetGroundTruthPose(ptransform_out)); -} - -ASC_DECL size_t Drone::GetRobotInfoCount(void) const { - return (pimpl_->GetRobotInfoCount()); -} - -ASC_DECL const char* Drone::GetRobotInfoName(int iname) const { - return (pimpl_->GetRobotInfoName(iname)); -} - -ASC_DECL const char* Drone::GetRobotTopic( - const std::string& str_info_name) const { - return (pimpl_->GetRobotTopic(str_info_name)); -} - -ASC_DECL size_t Drone::GetSensorCount(void) const { - return (pimpl_->GetSensorCount()); -} - -ASC_DECL size_t -Drone::GetSensorInfoCount(const std::string& str_sensor_name) const { - return (pimpl_->GetSensorInfoCount(str_sensor_name)); -} - -ASC_DECL const char* Drone::GetSensorInfoName( - const std::string& str_sensor_name, int iinfo) const { - return (pimpl_->GetSensorInfoName(str_sensor_name, iinfo)); -} - -ASC_DECL const char* Drone::GetSensorName(int iname) const { - return (pimpl_->GetSensorName(iname)); -} - -ASC_DECL const char* Drone::GetSensorTopic( - const std::string& str_sensor_name, - const std::string& str_info_name) const { - return (pimpl_->GetSensorTopic(str_sensor_name, str_info_name)); -} - -ASC_DECL AsyncResult Drone::GoHomeAsync( - float sec_timeout, float velocity, FnResponseCallback fnresponse_callback) { - return (pimpl_->GoHomeAsync(sec_timeout, velocity, fnresponse_callback)); -} - -ASC_DECL AsyncResult Drone::HoverAsync(FnResponseCallback fnresponse_callback) { - return (pimpl_->HoverAsync(fnresponse_callback)); -} - -ASC_DECL Status Drone::IsAPIControlEnabled(bool* pf_is_enabled_out) const { - RETURN_CATCH_STATUS(pimpl_->IsAPIControlEnabled(pf_is_enabled_out)); -} - -ASC_DECL AsyncResult Drone::LandAsync(float sec_timeout, - FnResponseCallback fnresponse_callback) { - return (pimpl_->LandAsync(sec_timeout, fnresponse_callback)); -} - -ASC_DECL AsyncResult Drone::MoveByVelocityAsync( - float v_north, float v_east, float v_down, float sec_duration, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - FnResponseCallback fnresponse_callback) { - return (pimpl_->MoveByVelocityAsync(v_north, v_east, v_down, sec_duration, - yaw_control_mode, yaw_is_rate, yaw, - fnresponse_callback)); -} - -ASC_DECL AsyncResult Drone::MoveByVelocityBodyFrameAsync( - float v_forward, float v_right, float v_down, float sec_duration, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - FnResponseCallback fnresponse_callback) { - return (pimpl_->MoveByVelocityBodyFrameAsync( - v_forward, v_right, v_down, sec_duration, yaw_control_mode, yaw_is_rate, - yaw, fnresponse_callback)); -} - -ASC_DECL AsyncResult Drone::MoveOnPathAsync( - const VecVector3& path, float velocity, float timeout_sec, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - float lookahead, float adaptive_lookahead, - FnResponseCallback fnresponse_callback) { - return (pimpl_->MoveOnPathAsync(path, velocity, timeout_sec, yaw_control_mode, - yaw_is_rate, yaw, lookahead, - adaptive_lookahead, fnresponse_callback)); -} - -ASC_DECL AsyncResult Drone::MoveToPositionAsync( - float north, float east, float down, float velocity, float timeout_sec, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - float lookahead, float adaptive_lookahead, - FnResponseCallback fnresponse_callback) { - return (pimpl_->MoveToPositionAsync( - north, east, down, velocity, timeout_sec, yaw_control_mode, yaw_is_rate, - yaw, lookahead, adaptive_lookahead, fnresponse_callback)); -} - -ASC_DECL Status Drone::SetPose(const Transform& transform, - bool reset_kinematics) { - RETURN_CATCH_STATUS(pimpl_->SetPose(transform, reset_kinematics)); -} - -ASC_DECL AsyncResult -Drone::TakeoffAsync(float sec_timeout, FnResponseCallback fnresponse_callback) { - return (pimpl_->TakeoffAsync(sec_timeout, fnresponse_callback)); -} - -Drone::Impl::Impl() noexcept - : pclient_(), - pworld_(), - str_drone_name_(), - str_parent_topic_(), - str_sensor_topic_(), - str_world_parent_topic_(), - umrobot_name_topic_(), - umsensor_name_uminfo_topic_(), - umsensor_name_vecinfo_(), - vecstr_robot_info_name_(), - vecstr_sensor_name_() {} - -Drone::Impl::~Impl() {} - -Status Drone::Impl::Arm(bool* pf_is_armed_out) { - return (RequestDrone("Arm", pf_is_armed_out)); -} - -Status Drone::Impl::CanArm(bool* pf_can_be_armed_out) const { - return (RequestDrone("CanArm", pf_can_be_armed_out)); -} - -Status Drone::Impl::CancelLastTask(bool* pf_task_is_canceled_out) { - TAsyncResult ar; - Status status; - - ar = pclient_->RequestPriorityAsync(str_parent_topic_ + "/CancelLastTask", json(), nullptr); - if ((status = ar.Wait()) == Status::OK) { - const Message& message = ar.GetResult(); - ResponseMessage response_message; - - response_message.Deserialize(message); - if (response_message.GetErrorCode() == 0) - *pf_task_is_canceled_out = response_message.GetResult().is_boolean() && static_cast(response_message.GetResult()); - else - { - status = Status::RejectedByServer; - - log.ErrorF( - "Server method \"CancelLastTask\" failed: error %d: %s", - response_message.GetErrorCode(), - static_cast(response_message.GetResult()["message"]) - .c_str()); - } - } - - return (status); -} - -Status Drone::Impl::DisableAPIControl(bool* pf_is_disabled_out) { - return (RequestDrone("DisableApiControl", pf_is_disabled_out)); -} - -Status Drone::Impl::Disarm(bool* pf_is_disarmed_out) { - return (RequestDrone("Disarm", pf_is_disarmed_out)); -} - -Status Drone::Impl::EnableAPIControl(bool* pf_is_enabled_out) { - return (RequestDrone("EnableApiControl", pf_is_enabled_out)); -} - -Status Drone::Impl::GetCameraRay(const std::string& str_camera_name, - ImageType image_type, int x, int y, - Pose* ppose_out) { - Status status; - json json_params = {{"camera_id", str_camera_name}, - {"image_type", (int)image_type}, - {"x", x}, - {"y", y}}; - json json_response; - - if ((status = RequestDrone("GetCameraRay", json_params, &json_response)) == - Status::OK) - *ppose_out = json_response; - - return (status); -} - -Status Drone::Impl::GetGroundTruthPose(Transform* ptransform_out) { - Status status; - json json_response; - - if ((status = RequestDrone("GetGroundTruthPose", &json_response)) == - Status::OK) - *ptransform_out = json_response; - - return (status); -} - -size_t Drone::Impl::GetRobotInfoCount(void) const { - return (vecstr_robot_info_name_.size()); -} - -const char* Drone::Impl::GetRobotInfoName(int iname) const { - const char* sz_ret = nullptr; - - if ((iname >= 0) && (iname < vecstr_robot_info_name_.size())) - sz_ret = vecstr_robot_info_name_[iname].c_str(); - - return (sz_ret); -} - -const char* Drone::Impl::GetRobotTopic(const std::string& str_info_name) const { - const char* sz_ret = nullptr; - auto it_robot = umrobot_name_topic_.find(str_info_name); - - if (it_robot != umrobot_name_topic_.end()) sz_ret = it_robot->second.c_str(); - - return (sz_ret); -} - -size_t Drone::Impl::GetSensorCount(void) const { - return (vecstr_sensor_name_.size()); -} - -size_t Drone::Impl::GetSensorInfoCount( - const std::string& str_sensor_name) const { - size_t cinfo = 0; - auto it = umsensor_name_vecinfo_.find(str_sensor_name); - - if (it != umsensor_name_vecinfo_.end()) cinfo = it->second.size(); - - return (cinfo); -} - -const char* Drone::Impl::GetSensorInfoName(const std::string& str_sensor_name, - int iinfo) const { - const char* sz_ret = nullptr; - auto it = umsensor_name_vecinfo_.find(str_sensor_name); - - if ((it != umsensor_name_vecinfo_.end()) && (iinfo >= 0) && - (iinfo < it->second.size())) { - sz_ret = it->second[iinfo].c_str(); - } - - return (sz_ret); -} - -const char* Drone::Impl::GetSensorName(int iname) const { - const char* sz_ret = nullptr; - - if ((iname >= 0) && (iname < vecstr_sensor_name_.size())) - sz_ret = vecstr_sensor_name_[iname].c_str(); - - return (sz_ret); -} - -const char* Drone::Impl::GetSensorTopic( - const std::string& str_sensor_name, - const std::string& str_info_name) const { - const char* sz_ret = nullptr; - auto it_sensor = umsensor_name_uminfo_topic_.find(str_sensor_name); - - if (it_sensor != umsensor_name_uminfo_topic_.end()) { - auto& uminfo_topic = it_sensor->second; - auto it_info = uminfo_topic.find(str_info_name); - - if (it_info != uminfo_topic.end()) sz_ret = it_info->second.c_str(); - } - - return (sz_ret); -} - -AsyncResult Drone::Impl::GoHomeAsync(float sec_timeout, float velocity, - FnResponseCallback fnresponse_callback) { - json json_params = { - {"timeout_sec", sec_timeout}, - {"velocity", velocity}, - }; - - return (RequestDroneAsync("GoHome", json_params, fnresponse_callback)); -} - -AsyncResult Drone::Impl::HoverAsync(FnResponseCallback fnresponse_callback) { - return (RequestDroneAsync("Hover", json(), fnresponse_callback)); -} - -Status Drone::Impl::Initialize(std::shared_ptr& pclient, - std::shared_ptr& pworld, - const std::string& str_drone_name) { - Status status = Status::OK; - - log.InfoF("Initalizing Drone '%s'...", str_drone_name.c_str()); - - pclient_ = pclient; - pworld_ = pworld; - str_drone_name_ = str_drone_name; - str_world_parent_topic_ = pworld->GetParentTopic(); - - try { - SetTopics(); - LogTopics(); - - log.InfoF("Drone '%s' initialized for World scene '%s'", - str_drone_name_.c_str(), str_world_parent_topic_.c_str()); - } catch (Status status_caught) { - status = status_caught; - } - - if (status != Status::OK) { - char sz_error[512]; - - GetStatusString(status, sz_error); - log.ErrorF("Failed to initialize drone '%s': %s", str_drone_name.c_str(), - sz_error); - } - - return (status); -} - -Status Drone::Impl::IsAPIControlEnabled(bool* pf_is_enabled_out) const { - return (RequestDrone("IsApiControlEnabled", pf_is_enabled_out)); -} - -AsyncResult Drone::Impl::LandAsync(float sec_timeout, - FnResponseCallback fnresponse_callback) { - json json_params = {{"timeout_sec", sec_timeout}}; - - return (RequestDroneAsync("Land", json_params, fnresponse_callback)); -} - -AsyncResult Drone::Impl::MoveByVelocityAsync( - float v_north, float v_east, float v_down, float sec_duration, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - FnResponseCallback fnresponse_callback) { - json json_params = { - {"vx", v_north}, - {"vy", v_east}, - {"vz", v_down}, - {"duration", sec_duration}, - {"drivetrain", yaw_control_mode}, - {"yaw_is_rate", yaw_is_rate}, - {"yaw", yaw}, - }; - - return ( - RequestDroneAsync("MoveByVelocity", json_params, fnresponse_callback)); -} - -AsyncResult Drone::Impl::MoveByVelocityBodyFrameAsync( - float v_forward, float v_right, float v_down, float sec_duration, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - FnResponseCallback fnresponse_callback) { - json json_params = { - {"vx", v_forward}, - {"vy", v_right}, - {"vz", v_down}, - {"duration", sec_duration}, - {"drivetrain", yaw_control_mode}, - {"yaw_is_rate", yaw_is_rate}, - {"yaw", yaw}, - }; - - return (RequestDroneAsync("MoveByVelocityBodyFrame", json_params, - fnresponse_callback)); -} - -AsyncResult Drone::Impl::MoveOnPathAsync( - const VecVector3& path, float velocity, float timeout_sec, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - float lookahead, float adaptive_lookahead, - FnResponseCallback fnresponse_callback) { - json json_params = { - {"path", path}, - {"velocity", velocity}, - {"timeout_sec", timeout_sec}, - {"drivetrain", yaw_control_mode}, - {"yaw_is_rate", yaw_is_rate}, - {"yaw", yaw}, - {"lookahead", lookahead}, - {"adaptive_lookahead", adaptive_lookahead}, - }; - - return (RequestDroneAsync("MoveOnPath", json_params, fnresponse_callback)); -} - -AsyncResult Drone::Impl::MoveToPositionAsync( - float north, float east, float down, float velocity, float timeout_sec, - YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, - float lookahead, float adaptive_lookahead, - FnResponseCallback fnresponse_callback) { - json json_params = { - {"x", north}, - {"y", east}, - {"z", down}, - {"velocity", velocity}, - {"timeout_sec", timeout_sec}, - {"drivetrain", yaw_control_mode}, - {"yaw_is_rate", yaw_is_rate}, - {"yaw", yaw}, - {"lookahead", lookahead}, - {"adaptive_lookahead", adaptive_lookahead}, - }; - - return ( - RequestDroneAsync("MoveToPosition", json_params, fnresponse_callback)); -} - -AsyncResult Drone::Impl::TakeoffAsync(float sec_timeout, - FnResponseCallback fnresponse_callback) { - json json_params = {{"timeout_sec", sec_timeout}}; - - return (RequestDroneAsync("Takeoff", json_params, fnresponse_callback)); -} - -void Drone::Impl::LogTopics(void) const { - log.Info("-------------------------------------------------"); - log.InfoF("The following topics can be subscribed to for robot '%s':", - str_drone_name_.c_str()); - for (auto& pair_sensor : umsensor_name_uminfo_topic_) { - for (auto& pair_info : pair_sensor.second) - log.InfoF(" sensors[\"%s\"][\"%s\"]", pair_sensor.first.c_str(), - pair_info.first.c_str()); - } - for (auto& pair_robot : umrobot_name_topic_) - log.InfoF(" robot_info[\"%s\"]", pair_robot.first.c_str()); - log.Info("-------------------------------------------------"); -} - -Status Drone::Impl::RequestDrone(const std::string str_method, - const json& json_params) const { - Status status; - Message message_response; - - status = pclient_->Request(str_parent_topic_ + "/" + str_method, json_params, - &message_response); - if (status == Status::OK) { - ResponseMessage response_message; - - response_message.Deserialize(message_response); - if (response_message.GetErrorCode() != 0) { - status = Status::RejectedByServer; - - log.ErrorF( - "Server method \"%s\" failed: error %d: %s", str_method.c_str(), - response_message.GetErrorCode(), - static_cast(response_message.GetResult()["message"]) - .c_str()); - } - } - - return (status); -} - -Status Drone::Impl::RequestDrone(const std::string str_method, - bool* pf_out) const { - return (RequestDrone(str_method, json(), pf_out)); -} - -Status Drone::Impl::RequestDrone(const std::string str_method, - json* pjson_out) const { - return (RequestDrone(str_method, json(), pjson_out)); -} - -Status Drone::Impl::RequestDrone(const std::string str_method, - const json& json_params, bool* pf_out) const { - json json_response; - Status status; - - status = RequestDrone(str_method, json_params, &json_response); - if (status == Status::OK) *pf_out = json_response; - - return (status); -} - -Status Drone::Impl::RequestDrone(const std::string str_method, - const json& json_params, - json* pjson_out) const { - Message message_response; - Status status; - - status = pclient_->Request(str_parent_topic_ + "/" + str_method, json_params, - &message_response); - if (status == Status::OK) { - ResponseMessage response_message; - - response_message.Deserialize(message_response); - if (response_message.GetErrorCode() == 0) - *pjson_out = response_message.GetResult(); - else { - status = Status::RejectedByServer; - - log.ErrorF( - "Server method \"%s\" failed: error %d: %s", str_method.c_str(), - response_message.GetErrorCode(), - static_cast(response_message.GetResult()["message"]) - .c_str()); - } - } - - return (status); -} - -AsyncResult Drone::Impl::RequestDroneAsync( - const std::string str_method, const json& json_params, - FnResponseCallback fnresponse_callback) const { - return (internal::AsyncResultMessageConverter(pclient_->RequestAsync( - str_parent_topic_ + "/" + str_method, json_params, fnresponse_callback))); -} - -Status Drone::Impl::SetPose(const Transform& transform, bool reset_kinematics) { - json json_params = {{"pose", transform}, - {"reset_kinematics", reset_kinematics}}; - - return (RequestDrone("SetPose", json_params)); -} - -void Drone::Impl::SetRobotInfoTopics(void) { - // TODO: Change this to construct this info from the published topics rather - // than - // hard coding it. - - const char* c_sz_robot_info_name[3] = { - "actual_pose", - "collision_info", - "rotor_info", - }; - - umrobot_name_topic_.clear(); - vecstr_robot_info_name_.clear(); - for (auto& sz : c_sz_robot_info_name) { - umrobot_name_topic_.insert( - std::make_pair(sz, str_parent_topic_ + "/" + sz)); - vecstr_robot_info_name_.push_back(sz); - } -} - -#ifdef ORIG_IMPL // Implementation as done in Drone.py -void Drone::Impl::SetSensorTopics(World* pworld) { - // TODO: Change this to construct this info from the published topics rather - // than - // extracting it from the configuration. - - static const std::unordered_map c_umcapture_name_setting = { - {0, "scene_camera"}, // TODO: rename scene_camera topic to rgb_camera - {1, "depth_camera"}, - {2, "depth_planar_camera"}, - {3, "segmentation_camera"}, - {4, "depth_vis_camera"}, - {5, "disparity_normalized_camera"}, - {6, "surface_normals_camera"}, - }; - - auto& json_config = pworld->GetConfiguration(); - json json_actor_config; - - auto fnAddSensorInfo = [this](std::string sensor_name, std::string info_name, - std::string topic_root_name) { - auto it = umsensor_name_uminfo_topic_.find(sensor_name); - - if (it == umsensor_name_uminfo_topic_.end()) { - auto pair = umsensor_name_uminfo_topic_.emplace(sensor_name, UMStrStr()); - - it = pair.first; - vecstr_sensor_name_.push_back(sensor_name); - umsensor_name_vecinfo_.emplace(sensor_name, VecStr()); - } - - it->second.insert( - std::make_pair(info_name, topic_root_name + "/" + info_name)); - umsensor_name_vecinfo_[sensor_name].push_back(info_name); - }; - - umsensor_name_uminfo_topic_.clear(); - umsensor_name_vecinfo_.clear(); - vecstr_sensor_name_.clear(); - - // Get the config for this drone - if (!json_config.is_null() && json_config["actors"].is_array()) { - for (auto& json_actor : json_config["actors"]) { - if (json_actor["name"] == str_drone_name_) { - json_actor_config = json_actor["robot-config"]; - break; - } - } - } - - if (json_actor_config.is_null()) { - log.ErrorF("Actor \"%s\" not found in the config", str_drone_name_.c_str()); - throw Status::NotFound; - } - - // Loop through the robot's sensors - if (!json_actor_config["sensors"].is_array()) return; - - for (auto& json_sensor : json_actor_config["sensors"]) { - // Add topic entries for each recognized sensor type - std::string str_name = json_sensor["id"]; - std::string str_sensor_type = json_sensor["type"]; - std::string str_sensor_topic_root = str_sensor_topic_ + "/" + str_name; - - if (str_sensor_type == "airspeed") - fnAddSensorInfo(str_name, "airspeed", str_sensor_topic_root); - else if (str_sensor_type == "barometer") - fnAddSensorInfo(str_name, "barometer", str_sensor_topic_root); - else if (str_sensor_type == "battery") - fnAddSensorInfo(str_name, "battery", str_sensor_topic_root); - else if (str_sensor_type == "camera") { - auto json_subcameras = json_sensor["capture-settings"]; - - if (json_subcameras.is_array()) { - for (auto& json_subcamera : json_subcameras) { - auto i_image_type = json_subcamera["image-type"]; - auto it = c_umcapture_name_setting.find(i_image_type); - - if (it != c_umcapture_name_setting.end()) { - auto str_image_type = it->second; - - fnAddSensorInfo(str_name, str_image_type, str_sensor_topic_root); - fnAddSensorInfo(str_name, str_image_type + "_info", - str_sensor_topic_root); - } - } - } - } else if (str_sensor_type == "distance-sensor") - fnAddSensorInfo(str_name, "distance-sensor", str_sensor_topic_root); - else if (str_sensor_type == "gps") - fnAddSensorInfo(str_name, "gps", str_sensor_topic_root); - else if (str_sensor_type == "imu") - fnAddSensorInfo(str_name, "imu_kinematics", str_sensor_topic_root); - else if (str_sensor_type == "lidar") - fnAddSensorInfo(str_name, "lidar", str_sensor_topic_root); - else if (str_sensor_type == "magnetometer") - fnAddSensorInfo(str_name, "magnetometer", str_sensor_topic_root); - else if (str_sensor_type == "radar") { - fnAddSensorInfo(str_name, "radar_detections", str_sensor_topic_root); - fnAddSensorInfo(str_name, "radar_tracks", str_sensor_topic_root); - } else { - log.ErrorF("Unknown sensor type \"%s\" found in config for sensor \"%s\"", - str_sensor_type.c_str(), str_name.c_str()); - throw Status::NotFound; - } - } -} -#endif // ORIG_IMPL - -void Drone::Impl::SetSensorTopics(World* /*pworld*/) { - bool fdrone_found = false; - std::regex re_drone("^/Sim/.+/robots/([^/]+)/.+$"); - std::regex re_sensor("^/Sim/.+/sensors/([^/]+)/([^/]+)$"); - auto vec_str_topic = pclient_->GetTopicInfo(); - std::unordered_set us_sensor; - - // Clear existing sensor lists - umsensor_name_uminfo_topic_.clear(); - umsensor_name_vecinfo_.clear(); - vecstr_sensor_name_.clear(); - - // Loop through the robot's sensor topics - for (auto& str_topic : vec_str_topic) { - std::smatch smatch_drone; - - // Ignore topics except those for this drone - if (std::regex_match(str_topic, smatch_drone, re_drone)) { - std::ssub_match ssub_match_drone = smatch_drone[1]; - - if (ssub_match_drone.str() == str_drone_name_) { - std::smatch smatch_sensor; - - fdrone_found = true; - - // Ignore topics except those for the sensors - if (std::regex_match(str_topic, smatch_sensor, re_sensor)) { - // Add topic entries for each recognized sensor type - std::string str_sensor_name = smatch_sensor[1].str(); - std::string str_sensor_info_name = smatch_sensor[2].str(); - - // Add sensor name to list - us_sensor.insert(str_sensor_name); - - // Add sensor info name to sensor's list - { - auto it_info = umsensor_name_vecinfo_.find(str_sensor_name); - - if (it_info == umsensor_name_vecinfo_.end()) { - auto pair = - umsensor_name_vecinfo_.emplace(str_sensor_name, VecStr()); - - it_info = pair.first; - } - it_info->second.push_back(str_sensor_info_name); - } - - // Add sensor info topic to sensor's list - { - auto it_um_info = umsensor_name_uminfo_topic_.find(str_sensor_name); - - if (it_um_info == umsensor_name_uminfo_topic_.end()) { - auto pair = umsensor_name_uminfo_topic_.insert( - std::make_pair(str_sensor_name, UMStrStr())); - - it_um_info = pair.first; - } - it_um_info->second.insert( - std::make_pair(str_sensor_info_name, str_topic)); - } - } - } - } - } - - if (!fdrone_found) { - log.ErrorF("Actor \"%s\" not found in the config", str_drone_name_.c_str()); - throw Status::NotFound; - } -} - -void Drone::Impl::SetTopics(void) { - str_parent_topic_ = str_world_parent_topic_ + "/robots/" + str_drone_name_; - str_sensor_topic_ = str_parent_topic_ + "/sensors"; - SetSensorTopics(pworld_.get()); - SetRobotInfoTopics(); -} - -} // namespace client -} // namespace projectairsim -} // namespace microsoft diff --git a/client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj b/client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj deleted file mode 100644 index b39f71e9..00000000 --- a/client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj +++ /dev/null @@ -1,280 +0,0 @@ - - - - - Debug - Win32 - - - Release w Symbols - Win32 - - - Release w Symbols - x64 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 16.0 - Win32Proj - {954f33c4-81f0-4eb6-8380-7d7406cb9606} - AirSimMessage - 10.0 - AirSimMessage.2019 - - - - StaticLibrary - true - v142 - Unicode - - - StaticLibrary - false - v142 - true - Unicode - - - StaticLibrary - false - v142 - true - Unicode - - - StaticLibrary - true - v142 - Unicode - - - StaticLibrary - false - v142 - true - Unicode - - - StaticLibrary - false - v142 - true - Unicode - - - - - - - - - - - - - - - - - - - - - - - - - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level4 - true - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - - - - - Level4 - true - true - true - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - true - true - - - - - Level4 - true - true - true - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - Disabled - ProgramDatabase - false - - - - - true - true - true - - - - - Level4 - true - _DEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - - - - - Level4 - true - true - true - NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - true - true - - - - - Level4 - true - true - true - NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - Disabled - ProgramDatabase - false - - - - - true - true - true - - - - - - - - - - - - - - - - - Create - Create - Create - Create - Create - Create - - - - - - - - - - - \ No newline at end of file diff --git a/client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj.filters b/client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj.filters deleted file mode 100644 index adcedce5..00000000 --- a/client/cpp/AirSimMessageLib/AirSimMessage_2019.vcxproj.filters +++ /dev/null @@ -1,69 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - \ No newline at end of file diff --git a/client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj b/client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj deleted file mode 100644 index 7f1a26fa..00000000 --- a/client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj +++ /dev/null @@ -1,280 +0,0 @@ - - - - - Debug - Win32 - - - Release w Symbols - Win32 - - - Release w Symbols - x64 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 16.0 - Win32Proj - {70A957FF-AA39-4fe2-A55B-EADEC72CC665} - AirSimMessage - 10.0 - AirSimMessage.2022 - - - - StaticLibrary - true - v143 - Unicode - - - StaticLibrary - false - v143 - true - Unicode - - - StaticLibrary - false - v143 - true - Unicode - - - StaticLibrary - true - v143 - Unicode - - - StaticLibrary - false - v143 - true - Unicode - - - StaticLibrary - false - v143 - true - Unicode - - - - - - - - - - - - - - - - - - - - - - - - - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - $(ProjectDir)..\libraries\$(Platform)\$(Configuration)\ - $(ProjectDir)..\..\..\build\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level4 - true - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - - - - - Level4 - true - true - true - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - true - true - - - - - Level4 - true - true - true - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - Disabled - ProgramDatabase - false - - - - - true - true - true - - - - - Level4 - true - _DEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - - - - - Level4 - true - true - true - NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\$(Configuration)\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - - - - - true - true - true - - - - - Level4 - true - true - true - NDEBUG;_LIB;%(PreprocessorDefinitions) - true - Use - pch.h - stdcpp17 - $(ProjectDir)include\AirSimMessage\;$(ProjectDir)..\..\..\;$(ProjectDir)..\..\..\core_sim\include\;$(ProjectDir)..\..\..\core_sim\src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\eigen3-src\;$(ProjectDir)..\..\..\build\win$(PlatformArchitecture)\Debug\_deps\nlohmann-install\;$(ProjectDir)..\..\..\build\Win$(PlatformArchitecture)\Debug\_deps\msgpack-src\include;%(AdditionalIncludeDirectories) - Disabled - ProgramDatabase - false - - - - - true - true - true - - - - - - - - - - - - - - - - - Create - Create - Create - Create - Create - Create - - - - - - - - - - - \ No newline at end of file diff --git a/client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj.filters b/client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj.filters deleted file mode 100644 index 082ffeee..00000000 --- a/client/cpp/AirSimMessageLib/AirSimMessage_2022.vcxproj.filters +++ /dev/null @@ -1,69 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - \ No newline at end of file diff --git a/client/cpp/AirSimMessageLib/framework.h b/client/cpp/AirSimMessageLib/framework.h deleted file mode 100644 index 3209b4ab..00000000 --- a/client/cpp/AirSimMessageLib/framework.h +++ /dev/null @@ -1,3 +0,0 @@ -#pragma once - -#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers diff --git a/client/cpp/AirSimMessageLib/pch.cpp b/client/cpp/AirSimMessageLib/pch.cpp deleted file mode 100644 index a3a52423..00000000 --- a/client/cpp/AirSimMessageLib/pch.cpp +++ /dev/null @@ -1,10 +0,0 @@ -// Copyright (C) Microsoft Corporation. -// Copyright (C) 2025 IAMAI CONSULTING CORP -// -// MIT License. All rights reserved. - -// pch.cpp: source file corresponding to the pre-compiled header - -#include "pch.h" - -// When you are using pre-compiled headers, this source file is necessary for compilation to succeed. diff --git a/client/cpp/CMakeLists.txt b/client/cpp/CMakeLists.txt new file mode 100644 index 00000000..80b7efb4 --- /dev/null +++ b/client/cpp/CMakeLists.txt @@ -0,0 +1,589 @@ +cmake_minimum_required(VERSION 3.20) + +set(PROJECTAIRSIM_CPP_CLIENT_VERSION 0.2.0) +project(ProjectAirSimCppClient VERSION ${PROJECTAIRSIM_CPP_CLIENT_VERSION} LANGUAGES CXX) + +include(FetchContent) +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) + +get_filename_component(PROJECTAIRSIM_ROOT "${CMAKE_CURRENT_LIST_DIR}/../.." ABSOLUTE) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Debug CACHE STRING "Build type" FORCE) +endif() + +if(NOT WIN32) + find_package(Threads REQUIRED) +endif() + +FetchContent_Declare( + nlohmann-json + URL https://raw.githubusercontent.com/nlohmann/json/v3.11.3/single_include/nlohmann/json.hpp + URL_HASH SHA256=9bea4c8066ef4a1c206b2be5a36302f8926f7fdc6087af5d20b417d0cf103ea6 + DOWNLOAD_NO_EXTRACT TRUE +) +FetchContent_GetProperties(nlohmann-json) +if(NOT nlohmann-json_POPULATED) + FetchContent_Populate(nlohmann-json) +endif() +set(JSON_INCLUDE_DIR "${nlohmann-json_SOURCE_DIR}") + +FetchContent_Declare( + msgpack + GIT_REPOSITORY https://github.com/msgpack/msgpack-c.git + GIT_TAG cpp-6.1.0 +) +FetchContent_GetProperties(msgpack) +if(NOT msgpack_POPULATED) + FetchContent_Populate(msgpack) +endif() +set(MSGPACK_INCLUDE_DIR "${msgpack_SOURCE_DIR}/include") +add_compile_definitions(MSGPACK_NO_BOOST) + +find_package(Eigen3 QUIET) +if(NOT Eigen3_FOUND) + if(EXISTS "/usr/include/eigen3/Eigen/Core") + set(EIGEN_INCLUDE_DIR "/usr/include/eigen3") + elseif(EXISTS "/usr/local/include/eigen3/Eigen/Core") + set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") + endif() + if(NOT EIGEN_INCLUDE_DIR) + FetchContent_Declare( + Eigen3 + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG 3.4.0 + ) + FetchContent_GetProperties(Eigen3) + if(NOT eigen3_POPULATED) + FetchContent_Populate(Eigen3) + endif() + set(EIGEN_INCLUDE_DIR "${eigen3_SOURCE_DIR}") + endif() +endif() +if(Eigen3_FOUND) + get_target_property(PROJECTAIRSIM_EIGEN_INCLUDE_DIR Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES) +else() + set(PROJECTAIRSIM_EIGEN_INCLUDE_DIR "${EIGEN_INCLUDE_DIR}") +endif() + +set(NNG_TESTS OFF CACHE BOOL "" FORCE) +set(NNG_TOOLS OFF CACHE BOOL "" FORCE) +set(BUILD_SHARED_LIBS OFF CACHE BOOL "" FORCE) +FetchContent_Declare( + nng + GIT_REPOSITORY https://github.com/nanomsg/nng.git + GIT_TAG v1.11 +) +FetchContent_GetProperties(nng) +if(NOT nng_POPULATED) + FetchContent_Populate(nng) +endif() +add_subdirectory(${nng_SOURCE_DIR} ${nng_BINARY_DIR} EXCLUDE_FROM_ALL) + +add_library(NNGI STATIC + NNGI/NNGI.cpp +) + +target_include_directories(NNGI + PUBLIC + $ + $ + $ + $ + $ +) + +target_link_libraries(NNGI + PUBLIC + nng +) + +target_compile_definitions(NNGI + PUBLIC + NNGI_STATIC_LIB + NNG_STATIC_LIB + PRIVATE + NNGI_EXPORTS +) + +add_library(ProjectAirSimMessageLib STATIC + ProjectAirSimMessageLib/src/image_message.cpp + ProjectAirSimMessageLib/src/jsonc.cpp + ProjectAirSimMessageLib/src/message.cpp + ProjectAirSimMessageLib/src/pose_stamped_message.cpp + ProjectAirSimMessageLib/src/request_message.cpp + ProjectAirSimMessageLib/src/response_failure_message.cpp + ProjectAirSimMessageLib/src/response_message.cpp + ProjectAirSimMessageLib/src/response_success_message.cpp +) + +target_include_directories(ProjectAirSimMessageLib + PUBLIC + $ + $ + $ + $ + $ + $ + $ + $ + $ + $ +) + +if(Eigen3_FOUND) + target_link_libraries(ProjectAirSimMessageLib PUBLIC Eigen3::Eigen) +else() + target_include_directories(ProjectAirSimMessageLib PUBLIC ${EIGEN_INCLUDE_DIR}) +endif() + +add_library(ProjectAirsimClient STATIC + ProjectAirsimClientLib/src/ProjectAirsimClient.cpp + ProjectAirsimClientLib/src/AsyncResult.cpp + ProjectAirsimClientLib/src/AsyncResultInternal.cpp + ProjectAirsimClientLib/src/Client.cpp + ProjectAirsimClientLib/src/Drone.cpp + ProjectAirsimClientLib/src/ICommChannel.cpp + ProjectAirsimClientLib/src/Log.cpp + ProjectAirsimClientLib/src/Message.cpp + ProjectAirsimClientLib/src/RefCounted.cpp + ProjectAirsimClientLib/src/Status.cpp + ProjectAirsimClientLib/src/Topics.cpp + ProjectAirsimClientLib/src/Types.cpp + ProjectAirsimClientLib/src/Utils.cpp + ProjectAirsimClientLib/src/World.cpp + ProjectAirsimClientLib/src/Rover.cpp + ProjectAirsimClientLib/src/EnvActor.cpp + ProjectAirsimClientLib/src/StaticSensorActor.cpp + ${PROJECTAIRSIM_ROOT}/core_sim/src/math_utils.cpp +) + +target_include_directories(ProjectAirsimClient + PUBLIC + $ + $ + $ + $ + $ + $ + $ + $ + $ + $ + $ + $ + $ + $ +) + +if(Eigen3_FOUND) + target_link_libraries(ProjectAirsimClient PUBLIC Eigen3::Eigen) +else() + target_include_directories(ProjectAirsimClient PUBLIC ${EIGEN_INCLUDE_DIR}) +endif() + +target_link_libraries(ProjectAirsimClient + PUBLIC + ProjectAirSimMessageLib + NNGI + nng +) + +if(WIN32) + target_link_libraries(ProjectAirsimClient + PUBLIC + Version + ) +endif() + +target_compile_definitions(ProjectAirsimClient + PUBLIC + PROJECTAIRSIMCLIENT_STATIC + NNGI_STATIC_LIB + NNG_STATIC_LIB + PRIVATE + PROJECTAIRSIMCLIENT_LIB_BUILD + MSGPACK_NO_BOOST +) + +if(WIN32) + set(PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB + "${CMAKE_CURRENT_BINARY_DIR}/ProjectAirSimCppClient.lib" + ) + add_custom_command( + OUTPUT ${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} + COMMAND ${CMAKE_COMMAND} -E remove -f ${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} + COMMAND ${CMAKE_AR} /NOLOGO /OUT:${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} + $ + $ + $ + $ + DEPENDS ProjectAirsimClient ProjectAirSimMessageLib NNGI nng + COMMENT "Creating merged ProjectAirSimCppClient static library" + VERBATIM + ) +else() + set(PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB + "${CMAKE_CURRENT_BINARY_DIR}/libProjectAirSimCppClient.a" + ) + set(PROJECTAIRSIM_CPP_CLIENT_MRI_SCRIPT + "${CMAKE_CURRENT_BINARY_DIR}/ProjectAirSimCppClient.mri" + ) + file(GENERATE + OUTPUT ${PROJECTAIRSIM_CPP_CLIENT_MRI_SCRIPT} + CONTENT +"CREATE ${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} +ADDLIB $ +ADDLIB $ +ADDLIB $ +ADDLIB $ +SAVE +END +" + ) + add_custom_command( + OUTPUT ${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} + COMMAND ${CMAKE_COMMAND} -E remove -f ${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} + COMMAND /bin/sh -c "\"$1\" -M < \"$2\"" sh ${CMAKE_AR} ${PROJECTAIRSIM_CPP_CLIENT_MRI_SCRIPT} + DEPENDS ProjectAirsimClient ProjectAirSimMessageLib NNGI nng ${PROJECTAIRSIM_CPP_CLIENT_MRI_SCRIPT} + COMMENT "Creating merged ProjectAirSimCppClient static library" + VERBATIM + ) +endif() + +add_custom_target(ProjectAirSimCppClientMerged ALL + DEPENDS ${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} +) + +add_library(FakeNNGI STATIC + tests/FakeNNGI.cpp +) + +target_compile_definitions(FakeNNGI + PUBLIC + NNGI_STATIC_LIB +) + +target_include_directories(FakeNNGI + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/tests + ${CMAKE_CURRENT_SOURCE_DIR}/NNGI/include + ${CMAKE_CURRENT_SOURCE_DIR}/NNGI/include/NNGI +) + +add_library(ProjectAirsimClientMocked STATIC + ProjectAirsimClientLib/src/ProjectAirsimClient.cpp + ProjectAirsimClientLib/src/AsyncResult.cpp + ProjectAirsimClientLib/src/AsyncResultInternal.cpp + ProjectAirsimClientLib/src/Client.cpp + ProjectAirsimClientLib/src/Drone.cpp + ProjectAirsimClientLib/src/ICommChannel.cpp + ProjectAirsimClientLib/src/Log.cpp + ProjectAirsimClientLib/src/Message.cpp + ProjectAirsimClientLib/src/RefCounted.cpp + ProjectAirsimClientLib/src/Status.cpp + ProjectAirsimClientLib/src/Topics.cpp + ProjectAirsimClientLib/src/Types.cpp + ProjectAirsimClientLib/src/Utils.cpp + ProjectAirsimClientLib/src/World.cpp + ProjectAirsimClientLib/src/Rover.cpp + ProjectAirsimClientLib/src/EnvActor.cpp + ProjectAirsimClientLib/src/StaticSensorActor.cpp + ${PROJECTAIRSIM_ROOT}/core_sim/src/math_utils.cpp +) + +target_include_directories(ProjectAirsimClientMocked + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/ProjectAirsimClientLib + ${CMAKE_CURRENT_SOURCE_DIR}/ProjectAirsimClientLib/Include/ProjectAirsimClient + ${CMAKE_CURRENT_SOURCE_DIR}/ProjectAirsimClientLib/Include + ${CMAKE_CURRENT_SOURCE_DIR}/ProjectAirSimMessageLib/include + ${CMAKE_CURRENT_SOURCE_DIR}/NNGI/include + ${PROJECTAIRSIM_ROOT} + ${PROJECTAIRSIM_ROOT}/core_sim/include + ${PROJECTAIRSIM_ROOT}/core_sim/src + ${JSON_INCLUDE_DIR} + ${MSGPACK_INCLUDE_DIR} +) + +if(Eigen3_FOUND) + target_link_libraries(ProjectAirsimClientMocked PUBLIC Eigen3::Eigen) +else() + target_include_directories(ProjectAirsimClientMocked PUBLIC ${EIGEN_INCLUDE_DIR}) +endif() + +target_link_libraries(ProjectAirsimClientMocked + PUBLIC + ProjectAirSimMessageLib + FakeNNGI +) + +if(WIN32) + target_link_libraries(ProjectAirsimClientMocked + PUBLIC + Version + ) +endif() + +target_compile_definitions(ProjectAirsimClientMocked + PUBLIC + PROJECTAIRSIMCLIENT_STATIC + NNGI_STATIC_LIB + NNG_STATIC_LIB + PRIVATE + PROJECTAIRSIMCLIENT_LIB_BUILD + MSGPACK_NO_BOOST +) + +add_executable(hello_drone + example_user_apps/HelloDrone/HelloDrone.cpp +) + +add_executable(cpp_client_scenarios + example_user_apps/CppClientScenarios/CppClientScenarios.cpp +) + +add_executable(user_scenario_template + example_user_apps/UserScenarioTemplate/UserScenarioTemplate.cpp +) + +add_executable(user_rover_scenario + example_user_apps/UserRoverScenario/UserRoverScenario.cpp +) + +add_executable(user_env_actor_scenario + example_user_apps/UserEnvActorScenario/UserEnvActorScenario.cpp +) + +add_executable(user_static_sensor_scenario + example_user_apps/UserStaticSensorScenario/UserStaticSensorScenario.cpp +) + +add_executable(user_lidar_scenario + example_user_apps/UserLidarScenario/UserLidarScenario.cpp +) + +add_executable(user_radar_scenario + example_user_apps/UserRadarScenario/UserRadarScenario.cpp +) + +add_executable(cpp_client_mocked_unit_tests + tests/CppClientUnitTests.cpp +) + +add_executable(cpp_client_unit_tests + tests/CppClientIntegrationTests.cpp +) + +target_link_libraries(hello_drone + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(cpp_client_scenarios + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(user_scenario_template + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(user_rover_scenario + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(user_env_actor_scenario + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(user_static_sensor_scenario + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(user_lidar_scenario + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(user_radar_scenario + PRIVATE + ProjectAirsimClient +) + +target_link_libraries(cpp_client_mocked_unit_tests + PRIVATE + ProjectAirsimClientMocked +) + +target_link_libraries(cpp_client_unit_tests + PRIVATE + ProjectAirsimClient +) + +if(NOT WIN32) + target_link_libraries(hello_drone PRIVATE Threads::Threads) + target_link_libraries(cpp_client_scenarios PRIVATE Threads::Threads) + target_link_libraries(user_scenario_template PRIVATE Threads::Threads) + target_link_libraries(user_rover_scenario PRIVATE Threads::Threads) + target_link_libraries(user_env_actor_scenario PRIVATE Threads::Threads) + target_link_libraries(user_static_sensor_scenario PRIVATE Threads::Threads) + target_link_libraries(user_lidar_scenario PRIVATE Threads::Threads) + target_link_libraries(user_radar_scenario PRIVATE Threads::Threads) + target_link_libraries(ProjectAirsimClientMocked PUBLIC Threads::Threads) + target_link_libraries(cpp_client_mocked_unit_tests PRIVATE Threads::Threads) + target_link_libraries(cpp_client_unit_tests PRIVATE Threads::Threads) +endif() + +# --------------------------------------------------------------------------------------------------------------------- +# +# Install and package support. +# +# --------------------------------------------------------------------------------------------------------------------- + +set(PROJECTAIRSIM_CPP_CLIENT_EXAMPLE_TARGETS + hello_drone + cpp_client_scenarios + user_scenario_template + user_rover_scenario + user_env_actor_scenario + user_static_sensor_scenario + user_lidar_scenario + user_radar_scenario +) + +install(FILES ${PROJECTAIRSIM_CPP_CLIENT_FINAL_LIB} + DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +install(TARGETS ${PROJECTAIRSIM_CPP_CLIENT_EXAMPLE_TARGETS} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +install(DIRECTORY ProjectAirsimClientLib/Include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + FILES_MATCHING PATTERN "*.h" +) + +install(DIRECTORY ProjectAirSimMessageLib/include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + FILES_MATCHING PATTERN "*.hpp" +) + +install(DIRECTORY NNGI/include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + FILES_MATCHING PATTERN "*.h" +) + +install(DIRECTORY ${PROJECTAIRSIM_ROOT}/core_sim/include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + FILES_MATCHING + PATTERN "*.h" + PATTERN "*.hpp" +) + +install(FILES ${JSON_INCLUDE_DIR}/json.hpp + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install(DIRECTORY ${MSGPACK_INCLUDE_DIR}/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + FILES_MATCHING + PATTERN "*.h" + PATTERN "*.hpp" + PATTERN "*.ipp" +) + +if(PROJECTAIRSIM_EIGEN_INCLUDE_DIR AND EXISTS "${PROJECTAIRSIM_EIGEN_INCLUDE_DIR}/Eigen") + install(DIRECTORY ${PROJECTAIRSIM_EIGEN_INCLUDE_DIR}/Eigen + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + ) +endif() + +install(DIRECTORY example_user_apps/ + DESTINATION examples + FILES_MATCHING + PATTERN "*.cpp" + PATTERN "*.md" + PATTERN "*.txt" + PATTERN "*.sln" EXCLUDE + PATTERN "*.vcxproj" EXCLUDE + PATTERN "*.filters" EXCLUDE +) + +install(DIRECTORY scripts/ + DESTINATION scripts + FILES_MATCHING PATTERN "*.sh" +) + +install(DIRECTORY ${PROJECTAIRSIM_ROOT}/client/python/example_user_scripts/sim_config/ + DESTINATION examples/sim_config + FILES_MATCHING + PATTERN "*.json" + PATTERN "*.jsonc" +) + +if(WIN32) + set(PROJECTAIRSIM_CPP_CLIENT_PACKAGE_PLATFORM "win64") + set(PROJECTAIRSIM_CPP_CLIENT_PACKAGE_GENERATOR "ZIP") + set(PROJECTAIRSIM_CPP_CLIENT_LIB_PREFIX "") + set(PROJECTAIRSIM_CPP_CLIENT_LIB_SUFFIX ".lib") +else() + set(PROJECTAIRSIM_CPP_CLIENT_PACKAGE_PLATFORM "linux64") + set(PROJECTAIRSIM_CPP_CLIENT_PACKAGE_GENERATOR "TGZ") + set(PROJECTAIRSIM_CPP_CLIENT_LIB_PREFIX "lib") + set(PROJECTAIRSIM_CPP_CLIENT_LIB_SUFFIX ".a") +endif() + +set(PROJECTAIRSIM_CPP_CLIENT_PACKAGE_NAME + "ProjectAirSimCppClient-${PROJECT_VERSION}-${PROJECTAIRSIM_CPP_CLIENT_PACKAGE_PLATFORM}" +) + +configure_package_config_file( + packaging/ProjectAirSimCppClientConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/ProjectAirSimCppClientConfig.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/ProjectAirSimCppClient +) + +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/ProjectAirSimCppClientConfigVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion +) + +configure_file( + packaging/README.md.in + ${CMAKE_CURRENT_BINARY_DIR}/README.md + @ONLY +) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/ProjectAirSimCppClientConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/ProjectAirSimCppClientConfigVersion.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/ProjectAirSimCppClient +) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/README.md + ${PROJECTAIRSIM_ROOT}/docs/license.md + DESTINATION . +) + +set(CPACK_PACKAGE_NAME ProjectAirSimCppClient) +set(CPACK_PACKAGE_VENDOR "IAMAI CONSULTING CORP") +set(CPACK_PACKAGE_VERSION ${PROJECT_VERSION}) +set(CPACK_PACKAGE_FILE_NAME ${PROJECTAIRSIM_CPP_CLIENT_PACKAGE_NAME}) +set(CPACK_PACKAGE_DIRECTORY "${PROJECTAIRSIM_ROOT}/packages/projectairsim_cpp_client") +set(CPACK_GENERATOR ${PROJECTAIRSIM_CPP_CLIENT_PACKAGE_GENERATOR}) +set(CPACK_INCLUDE_TOPLEVEL_DIRECTORY ON) +set(CPACK_PACKAGE_CHECKSUM SHA256) +include(CPack) diff --git a/client/cpp/NNGI/framework.h b/client/cpp/NNGI/framework.h index 54b83e94..dd69e078 100644 --- a/client/cpp/NNGI/framework.h +++ b/client/cpp/NNGI/framework.h @@ -1,5 +1,6 @@ #pragma once +#ifdef _WIN32 #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers -// Windows Header Files #include +#endif diff --git a/client/cpp/NNGI/include/NNGI/NNGI.h b/client/cpp/NNGI/include/NNGI/NNGI.h index 0438b45c..0d2a1ce4 100644 --- a/client/cpp/NNGI/include/NNGI/NNGI.h +++ b/client/cpp/NNGI/include/NNGI/NNGI.h @@ -1,6 +1,7 @@ #pragma once #include "NNGIDecl.h" #include +#include @@ -64,11 +65,11 @@ enum nng_errno_enum { NNGI_DECL int nng_close(nng_socket s); NNGI_DECL int nng_dial(nng_socket sid, const char *addr, nng_dialer *dp, int flags); -NNGI_DECL void nng_free(void *buf, size_t sz); +NNGI_DECL void nng_free(void *buf, std::size_t sz); NNGI_DECL int nng_pair0_open(nng_socket *); -NNGI_DECL int nng_recv(nng_socket s, void *buf, size_t *szp, int flags); +NNGI_DECL int nng_recv(nng_socket s, void *buf, std::size_t *szp, int flags); NNGI_DECL int nng_req0_open(nng_socket *); -NNGI_DECL int nng_send(nng_socket s, void *buf, size_t len, int flags); +NNGI_DECL int nng_send(nng_socket s, void *buf, std::size_t len, int flags); NNGI_DECL int nng_socket_set_ms(nng_socket id, const char *n, nng_duration v); NNGI_DECL const char *nng_strerror(int); NNGI_DECL const char* nng_version(void); diff --git a/client/cpp/NNGI/include/NNGI/NNGIDecl.h b/client/cpp/NNGI/include/NNGI/NNGIDecl.h index 0802c71f..3d7b6b8f 100644 --- a/client/cpp/NNGI/include/NNGI/NNGIDecl.h +++ b/client/cpp/NNGI/include/NNGI/NNGIDecl.h @@ -1,8 +1,17 @@ #pragma once #ifndef NNGI_DECL -#define NNGI_DECL __declspec(dllexport) +#ifdef NNGI_STATIC_LIB + #define NNGI_DECL #else -#define NNGI_DECL __declspec(dllimport) +#ifdef _WIN32 + #ifdef NNGI_EXPORTS + #define NNGI_DECL __declspec(dllexport) + #else + #define NNGI_DECL __declspec(dllimport) + #endif +#else + #define NNGI_DECL +#endif +#endif #endif //NNGI_DECL - diff --git a/client/cpp/AirSimMessageLib/include/AirSimMessage/image_message.hpp b/client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/image_message.hpp similarity index 100% rename from client/cpp/AirSimMessageLib/include/AirSimMessage/image_message.hpp rename to client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/image_message.hpp diff --git a/client/cpp/AirSimMessageLib/include/AirSimMessage/jsonc.hpp b/client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/jsonc.hpp similarity index 100% rename from client/cpp/AirSimMessageLib/include/AirSimMessage/jsonc.hpp rename to client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/jsonc.hpp diff --git a/client/cpp/AirSimMessageLib/include/AirSimMessage/pose_stamped_message.hpp b/client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/pose_stamped_message.hpp similarity index 100% rename from client/cpp/AirSimMessageLib/include/AirSimMessage/pose_stamped_message.hpp rename to client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/pose_stamped_message.hpp diff --git a/client/cpp/AirSimMessageLib/include/AirSimMessage/request_message.hpp b/client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/request_message.hpp similarity index 100% rename from client/cpp/AirSimMessageLib/include/AirSimMessage/request_message.hpp rename to client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/request_message.hpp diff --git a/client/cpp/AirSimMessageLib/include/AirSimMessage/response_message.hpp b/client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/response_message.hpp similarity index 98% rename from client/cpp/AirSimMessageLib/include/AirSimMessage/response_message.hpp rename to client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/response_message.hpp index 7cf5023f..6b6938d0 100644 --- a/client/cpp/AirSimMessageLib/include/AirSimMessage/response_message.hpp +++ b/client/cpp/ProjectAirSimMessageLib/include/ProjectAirSimMessage/response_message.hpp @@ -4,6 +4,7 @@ // MIT License. All rights reserved. #pragma once +#include #include diff --git a/client/cpp/AirSimMessageLib/pch.h b/client/cpp/ProjectAirSimMessageLib/pch.h similarity index 93% rename from client/cpp/AirSimMessageLib/pch.h rename to client/cpp/ProjectAirSimMessageLib/pch.h index 9a3529e9..adb5fb68 100644 --- a/client/cpp/AirSimMessageLib/pch.h +++ b/client/cpp/ProjectAirSimMessageLib/pch.h @@ -12,9 +12,6 @@ #ifndef PCH_H #define PCH_H -// add headers that you want to pre-compile here -#include "framework.h" - #include "pose_stamped_message.hpp" #include "request_message.hpp" #include "response_message.hpp" diff --git a/client/cpp/AirSimMessageLib/src/image_message.cpp b/client/cpp/ProjectAirSimMessageLib/src/image_message.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/image_message.cpp rename to client/cpp/ProjectAirSimMessageLib/src/image_message.cpp diff --git a/client/cpp/AirSimMessageLib/src/jsonc.cpp b/client/cpp/ProjectAirSimMessageLib/src/jsonc.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/jsonc.cpp rename to client/cpp/ProjectAirSimMessageLib/src/jsonc.cpp diff --git a/client/cpp/AirSimMessageLib/src/message.cpp b/client/cpp/ProjectAirSimMessageLib/src/message.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/message.cpp rename to client/cpp/ProjectAirSimMessageLib/src/message.cpp diff --git a/client/cpp/AirSimMessageLib/src/pose_stamped_message.cpp b/client/cpp/ProjectAirSimMessageLib/src/pose_stamped_message.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/pose_stamped_message.cpp rename to client/cpp/ProjectAirSimMessageLib/src/pose_stamped_message.cpp diff --git a/client/cpp/AirSimMessageLib/src/request_message.cpp b/client/cpp/ProjectAirSimMessageLib/src/request_message.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/request_message.cpp rename to client/cpp/ProjectAirSimMessageLib/src/request_message.cpp diff --git a/client/cpp/AirSimMessageLib/src/response_failure_message.cpp b/client/cpp/ProjectAirSimMessageLib/src/response_failure_message.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/response_failure_message.cpp rename to client/cpp/ProjectAirSimMessageLib/src/response_failure_message.cpp diff --git a/client/cpp/AirSimMessageLib/src/response_message.cpp b/client/cpp/ProjectAirSimMessageLib/src/response_message.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/response_message.cpp rename to client/cpp/ProjectAirSimMessageLib/src/response_message.cpp diff --git a/client/cpp/AirSimMessageLib/src/response_success_message.cpp b/client/cpp/ProjectAirSimMessageLib/src/response_success_message.cpp similarity index 100% rename from client/cpp/AirSimMessageLib/src/response_success_message.cpp rename to client/cpp/ProjectAirSimMessageLib/src/response_success_message.cpp diff --git a/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ASCDecl.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ASCDecl.h new file mode 100644 index 00000000..5b83cad4 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ASCDecl.h @@ -0,0 +1,21 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once + +#ifndef ASC_DECL +#ifdef PROJECTAIRSIMCLIENT_STATIC + #define ASC_DECL +#else +#ifdef _WIN32 + #ifdef PROJECTAIRSIMCLIENT_LIB_BUILD + #define ASC_DECL __declspec(dllexport) + #else + #define ASC_DECL __declspec(dllimport) + #endif // PROJECTAIRSIMCLIENT_LIB_BUILD +#else + #define ASC_DECL +#endif +#endif // PROJECTAIRSIMCLIENT_STATIC +#endif // ASC_DECL diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/AsyncResult.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/AsyncResult.h similarity index 100% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/AsyncResult.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/AsyncResult.h diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/Client.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Client.h similarity index 86% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/Client.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Client.h index c27f7192..017c443a 100644 --- a/client/cpp/AirSimClientDLL/Include/AirSimClient/Client.h +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Client.h @@ -4,7 +4,7 @@ // MIT License. All rights reserved. #pragma once -#include +#include #include #include #include @@ -49,6 +49,8 @@ class Client { ASC_DECL std::vector GetTopicInfo(void); ASC_DECL Status Subscribe(const char* sz_topic_name, TopicCallback topic_callback); + ASC_DECL Status Publish(const char* sz_topic_name, + const nlohmann::json& json_message); ASC_DECL Status Unsubscribe(const char* sz_topic_name); ASC_DECL Status Unsubscribe(const std::vector& vec_topic_name); ASC_DECL Status UnsubscribeAll(void) noexcept; @@ -58,6 +60,10 @@ class Client { ASC_DECL Status Request(const std::string& str_method, const nlohmann::json& json_parameters, client::Message* pmessage_response_out) noexcept; + ASC_DECL Status SetInteractiveFeature(const std::string& str_feature_name, + bool f_enable, + bool* pf_enabled_out); + ASC_DECL Status GetBuildCommitHash(std::string* pstr_hash_out); ASC_DECL TAsyncResult RequestAsync( const std::string& str_method, const nlohmann::json& json_parameters, FnResponseCallback fnresponsecallback = nullptr) noexcept; diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/Common.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Common.h similarity index 100% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/Common.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Common.h diff --git a/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Drone.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Drone.h new file mode 100644 index 00000000..98e404d6 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Drone.h @@ -0,0 +1,251 @@ +// Copyright (C) Microsoft Corporation. +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once +#include +#include + +#include "ASCDecl.h" +#include "Client.h" +#include "World.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class Drone { + public: + // Camera sensor image types + enum class ImageType : int { + Scene = 0, + DepthPlanar = 1, + DepthPerspective = 2, + Segmentation = 3, + DepthVis = 4, + DisparityNormalized = 5, + SurfaceNormals = 6, + // kInfrared = 7, // this type not implemented yet + + // The following must be last + MAX + }; + + // Yaw control modes + enum class YawControlMode { + MaxDegreeOfFreedom = + 0, // Yaw angle, if specified, is relative to initial yaw + ForwardOnly = + 1, // Yaw angle, if specified, is relative to direction of travel + }; // enum class YawControlMode + + // VTOL flight modes + enum class VTOLMode { + Multirotor = 0, // Multirotor (helicopter) mode + FixedWing = 1, // Fixed-wing mode when possible, multirotor otherwise + }; // enum class VTOLMode + + public: + // Timeout value meaning no timeout + ASC_DECL static const float kNoTimeout; + + public: + ASC_DECL Drone(void) noexcept; + ASC_DECL ~Drone(); + + // Initialize the drone object. Connects to named drone in + // simulation connected to the client and in the specified world. + // + // Arguments: + // pclient Pointer to client object + // pworld Pointer to world object + // drone_name Name of the drone to which the object attaches + // + // Returns: + // (Return) Initialization status + ASC_DECL Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& drone_name); + + // API control + ASC_DECL Status CancelLastTask(bool* pf_task_is_canceled_out); + ASC_DECL Status DisableAPIControl(bool* pf_is_disabled_out); + ASC_DECL Status EnableAPIControl(bool* pf_is_enabled_out); + ASC_DECL Status IsAPIControlEnabled(bool* pf_is_enabled_out) const; + + // Arming control + ASC_DECL Status Arm(bool* pf_is_armed_out); + ASC_DECL Status CanArm(bool* pf_can_be_armed_out) const; + ASC_DECL Status Disarm(bool* pf_is_disarmed_out); + ASC_DECL Status GetReadyState(ReadyState* pready_state_out) const; + ASC_DECL Status GetLandedState(LandedState* planded_state_out) const; + + // Topic names + ASC_DECL size_t GetRobotInfoCount(void) const; + ASC_DECL const char* GetRobotInfoName(int iname) const; + ASC_DECL const char* GetRobotTopic(const std::string& str_info_name) const; + ASC_DECL size_t GetSensorCount(void) const; + ASC_DECL size_t GetSensorInfoCount(const std::string& str_sensor_name) const; + ASC_DECL const char* GetSensorInfoName(const std::string& str_sensor_name, + int iinfo) const; + ASC_DECL const char* GetSensorName(int iname) const; + ASC_DECL const char* GetSensorTopic(const std::string& str_sensor_name, + const std::string& str_info_name) const; + + // Sensors + ASC_DECL Status GetCameraRay(const std::string& str_camera_name, + ImageType image_type, int x, int y, + Pose* ppose_out); + ASC_DECL Status GetIMUData(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetGPSData(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetAirspeedData(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetBarometerData(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetMagnetometerData(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetLidarData(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetRadarDetections(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetRadarTracks(const std::string& str_sensor_name, + json* pjson_out) const; + ASC_DECL Status GetBatteryState(json* pjson_out) const; + ASC_DECL Status SetBatteryRemaining(float desired_battery_remaining, + bool* pf_is_set_out); + ASC_DECL Status GetBatteryDrainRate(float* pf_drain_rate_out) const; + ASC_DECL Status SetBatteryDrainRate(float desired_drain_rate, + bool* pf_is_set_out); + ASC_DECL Status SetBatteryHealthStatus(bool is_desired_state_healthy, + bool* pf_is_set_out); + ASC_DECL Status GetImages(const std::string& str_camera_id, + const std::vector& veci_image_type_id, + json* pjson_out) const; + ASC_DECL Status CameraLookAtObject(const std::string& str_camera_id, + const std::string& str_object_name, + bool wait_for_pose_update, + bool* pf_success_out); + ASC_DECL Status CameraDrawFrustum(const std::string& str_camera_id, + bool to_enable, ImageType image_type, + bool* pf_success_out); + ASC_DECL Status SetCameraPose(const std::string& str_camera_id, + const json& json_pose, + bool wait_for_pose_update, + bool* pf_success_out); + ASC_DECL Status ResetCameraPose(const std::string& str_camera_id, + bool wait_for_pose_update, + bool* pf_success_out); + ASC_DECL Status SetFocalLength(const std::string& str_camera_id, + int image_type_id, float focal_length, + bool* pf_success_out); + ASC_DECL Status SetDepthOfFieldTransitionThreshold( + const std::string& str_camera_id, int image_type_id, + float transition_threshold, bool* pf_success_out); + ASC_DECL Status SetDepthOfFieldFocalRegion(const std::string& str_camera_id, + int image_type_id, + float max_focal_distance, + bool* pf_success_out); + ASC_DECL Status SetChromaticAberrationIntensity( + const std::string& str_camera_id, int image_type_id, float intensity, + bool* pf_success_out); + ASC_DECL Status SetFieldOfView(const std::string& str_camera_id, + int image_type_id, float field_of_view, + bool* pf_success_out); + + // Drone state + ASC_DECL Status GetGroundTruthPose(Transform* ptransform_out); + ASC_DECL Status GetGroundTruthGeoLocation(GeoPosition* pgeo_position_out); + ASC_DECL Status GetEstimatedGeoLocation(GeoPosition* pgeo_position_out); + ASC_DECL Status GetGroundTruthKinematics(json* pjson_out) const; + ASC_DECL Status GetEstimatedKinematics(json* pjson_out) const; + ASC_DECL Status SetGroundTruthKinematics(const json& json_kinematics, + bool* pf_success_out); + ASC_DECL Status SetPose(const Transform& transform, bool reset_kinematics); + ASC_DECL Status SetControlSignals(const json& json_control_signal_map, + bool* pf_success_out); + ASC_DECL Status UpdateActuatorFaultState(const std::string& actuator_id, + bool fault_configured, + bool* pf_success_out); + ASC_DECL Status SetExternalForce(const std::vector& ext_force, + bool* pf_success_out); + + // Take-off and landing + ASC_DECL AsyncResult + LandAsync(float sec_timeout = kNoTimeout, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult + TakeoffAsync(float sec_timeout = kNoTimeout, + FnResponseCallback fnresponse_callback = nullptr); + + // Movement + ASC_DECL AsyncResult + GoHomeAsync(float sec_timeout = kNoTimeout, float velocity = 0.5f, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult + HoverAsync(FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult + RequestControlAsync(FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult + SetMissionModeAsync(FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult + SetVTOLModeAsync(VTOLMode vtol_mode, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult MoveByVelocityAsync( + float v_north, float v_east, float v_down, float sec_duration, + YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, + bool yaw_is_rate = true, float yaw = 0.0f, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult MoveByVelocityZAsync( + float v_north, float v_east, float z, float sec_duration, + YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, + bool yaw_is_rate = true, float yaw = 0.0f, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult MoveByVelocityBodyFrameAsync( + float v_forward, float v_right, float v_down, float sec_duration, + YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, + bool yaw_is_rate = true, float yaw = 0.0f, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult MoveByVelocityBodyFrameZAsync( + float v_forward, float v_right, float z, float sec_duration, + YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, + bool yaw_is_rate = true, float yaw = 0.0f, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult MoveByHeadingAsync( + float heading, float speed, float v_down, float sec_duration, + float heading_margin = 0.08726646259971647f, float yaw_rate = 0.0f, + float timeout_sec = kNoTimeout, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult MoveOnPathAsync( + const VecVector3& path, float velocity, float timeout_sec = kNoTimeout, + YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, + bool yaw_is_rate = true, float yaw = 0.0f, float lookahead = -1, + float adaptive_lookahead = 1.0f, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult MoveToPositionAsync( + float north, float east, float down, float velocity, + float timeout_sec = kNoTimeout, + YawControlMode yaw_control_mode = YawControlMode::MaxDegreeOfFreedom, + bool yaw_is_rate = true, float yaw = 0.0, float lookahead = -1.0, + float adaptive_lookahead = 1.0, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult RotateToYawAsync( + float yaw, float timeout_sec = kNoTimeout, + float margin = 0.08726646259971647f, float yaw_rate = 0.0f, + FnResponseCallback fnresponse_callback = nullptr); + ASC_DECL AsyncResult RotateByYawRateAsync( + float yaw_rate, float sec_duration, + FnResponseCallback fnresponse_callback = nullptr); + + protected: + class Impl; // Drone implementation + + protected: + std::shared_ptr pimpl_; // Pointer to implementation +}; // class Drone + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/EnvActor.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/EnvActor.h new file mode 100644 index 00000000..08495c19 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/EnvActor.h @@ -0,0 +1,81 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once +#include +#include +#include + +#include "ASCDecl.h" +#include "Client.h" +#include "Status.h" +#include "Types.h" +#include "World.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class EnvActor { + public: + ASC_DECL EnvActor(void) noexcept; + ASC_DECL ~EnvActor(); + + // Initialize the env actor object. + // + // Arguments: + // pclient Pointer to client object + // pworld Pointer to world object + // actor_name Name of the environment actor in the scene + // + // Returns: + // (Return) Initialization status + ASC_DECL Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& actor_name); + + // Assign a trajectory to this actor. + // + // Arguments: + // traj_name Name of the trajectory asset + // to_loop Whether to loop the trajectory + // time_offset Time offset into the trajectory (seconds) + // x/y/z_offset Position offset (meters) + // roll/pitch/yaw_offset Orientation offset (radians) + ASC_DECL Status SetTrajectory(const std::string& traj_name, + bool to_loop = false, float time_offset = 0.0f, + float x_offset = 0.0f, float y_offset = 0.0f, + float z_offset = 0.0f, + float roll_offset = 0.0f, + float pitch_offset = 0.0f, + float yaw_offset = 0.0f); + + // Rotate a single link to the specified angle. + ASC_DECL Status SetLinkRotationAngle(const std::string& link_name, + float angle_deg); + + // Rotate a single link at the specified rate. + ASC_DECL Status SetLinkRotationRate(const std::string& link_name, + float rotation_deg_per_sec); + + // Rotate multiple links to specified angles. + // rotation_angle_map: map from link name to angle in degrees + ASC_DECL Status SetLinkRotationAngles( + const std::map& rotation_angle_map); + + // Rotate multiple links at specified rates. + // rotation_rate_map: map from link name to degrees/second + ASC_DECL Status SetLinkRotationRates( + const std::map& rotation_rate_map); + + protected: + class Impl; + + protected: + std::unique_ptr pimpl_; +}; // class EnvActor + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/IRefCounted.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/IRefCounted.h similarity index 100% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/IRefCounted.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/IRefCounted.h diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/Log.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Log.h similarity index 100% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/Log.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Log.h diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/Message.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Message.h similarity index 100% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/Message.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Message.h diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/AirSimClient.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ProjectAirsimClient.h similarity index 92% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/AirSimClient.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ProjectAirsimClient.h index c9d21929..865052bc 100644 --- a/client/cpp/AirSimClientDLL/Include/AirSimClient/AirSimClient.h +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ProjectAirsimClient.h @@ -13,6 +13,9 @@ #include "Status.h" #include "Types.h" #include "World.h" +#include "EnvActor.h" +#include "Rover.h" +#include "StaticSensorActor.h" namespace microsoft { namespace projectairsim { diff --git a/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Rover.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Rover.h new file mode 100644 index 00000000..cd0e3a49 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Rover.h @@ -0,0 +1,84 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once +#include +#include +#include + +#include "ASCDecl.h" +#include "AsyncResult.h" +#include "Client.h" +#include "Status.h" +#include "Types.h" +#include "World.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class Rover { + public: + // Timeout value meaning no timeout + ASC_DECL static const float kNoTimeout; + + public: + ASC_DECL Rover(void) noexcept; + ASC_DECL ~Rover(); + + // Initialize the rover object. Connects to named rover in + // simulation connected to the client and in the specified world. + // + // Arguments: + // pclient Pointer to client object + // pworld Pointer to world object + // rover_name Name of the rover to which the object attaches + // + // Returns: + // (Return) Initialization status + ASC_DECL Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& rover_name); + + // API control + ASC_DECL Status CancelLastTask(bool* pf_task_is_canceled_out); + ASC_DECL Status DisableAPIControl(bool* pf_is_disabled_out); + ASC_DECL Status EnableAPIControl(bool* pf_is_enabled_out); + ASC_DECL Status IsAPIControlEnabled(bool* pf_is_enabled_out) const; + + // Arming control + ASC_DECL Status Arm(bool* pf_is_armed_out); + ASC_DECL Status CanArm(bool* pf_can_be_armed_out) const; + ASC_DECL Status Disarm(bool* pf_is_disarmed_out); + + // State + ASC_DECL Status GetGroundTruthKinematics(json* pjson_out) const; + ASC_DECL Status SetPose(const Transform& transform, bool reset_kinematics); + + // Movement + ASC_DECL AsyncResult MoveToPositionAsync( + float north, float east, float velocity, + float timeout_sec = kNoTimeout, float yaw_rate_max = -1.0f, + float lookahead = -1.0f, float adaptive_lookahead = 1.0f, + FnResponseCallback fnresponse_callback = nullptr); + + ASC_DECL AsyncResult MoveByHeadingAsync( + float heading, float speed, float sec_duration = 3.0f, + float heading_margin = 0.08726646259971647f, // 5 deg in radians + float yaw_rate = 5.0f, float timeout_sec = kNoTimeout, + FnResponseCallback fnresponse_callback = nullptr); + + ASC_DECL Status SetRoverControls(float engine, float steering_angle, + float brake); + + protected: + class Impl; + + protected: + std::unique_ptr pimpl_; +}; // class Rover + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/StaticSensorActor.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/StaticSensorActor.h new file mode 100644 index 00000000..58412084 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/StaticSensorActor.h @@ -0,0 +1,59 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once +#include +#include +#include + +#include "ASCDecl.h" +#include "Client.h" +#include "Status.h" +#include "Types.h" +#include "World.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +// A static (non-moving, non-controlled) sensor platform. +// Supports sensor data queries but no movement or control methods. +class StaticSensorActor { + public: + ASC_DECL StaticSensorActor(void) noexcept; + ASC_DECL ~StaticSensorActor(); + + // Initialize the static sensor actor object. + // + // Arguments: + // pclient Pointer to client object + // pworld Pointer to world object + // actor_name Name of the static sensor actor in the scene + // + // Returns: + // (Return) Initialization status + ASC_DECL Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& actor_name); + + // Retrieve images from a camera sensor. + // + // Arguments: + // str_camera_id Camera sensor ID + // veci_image_type_id List of image type IDs to request + // pjson_out Output JSON with image data + ASC_DECL Status GetImages(const std::string& str_camera_id, + const std::vector& veci_image_type_id, + json* pjson_out) const; + + protected: + class Impl; + + protected: + std::unique_ptr pimpl_; +}; // class StaticSensorActor + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/Status.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Status.h similarity index 98% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/Status.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Status.h index 6c2d9b6b..a8b48f97 100644 --- a/client/cpp/AirSimClientDLL/Include/AirSimClient/Status.h +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Status.h @@ -4,6 +4,7 @@ // MIT License. All rights reserved. #pragma once +#include #include #include "ASCDecl.h" diff --git a/client/cpp/AirSimClientDLL/Include/AirSimClient/Types.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Types.h similarity index 91% rename from client/cpp/AirSimClientDLL/Include/AirSimClient/Types.h rename to client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Types.h index df2704db..43f9bd85 100644 --- a/client/cpp/AirSimClientDLL/Include/AirSimClient/Types.h +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/Types.h @@ -59,6 +59,20 @@ struct ColorRGBA { typedef std::vector VecColorRGBA; +// Landed state enum values +enum class LandedState { + Landed = 0, + Flying = 1, +}; + +// Vehicle readiness status +struct ReadyState { + bool is_ready; // true if vehicle is ready + std::string message; // readiness status message + + ASC_DECL ReadyState(void) noexcept; +}; // struct ReadyState + // Geographic position struct GeoPosition { float altitude; // Altitude (meters) diff --git a/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/World.h b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/World.h new file mode 100644 index 00000000..9d2a64c2 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/World.h @@ -0,0 +1,312 @@ +// Copyright (C) Microsoft Corporation. +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once +#include "ASCDecl.h" +#include "Client.h" +#include "Common.h" +#include "Status.h" +#include "Types.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class World { + public: + ASC_DECL World(void) noexcept; + ASC_DECL ~World(); + + // Initializes the world. If a scene config file name is given, the + // scene is loaded into the simulation. If a scene config file name is + // not given, this object reflects scene currently loaded into the + // simulator. (Note that the scene configuration JSON will not be + // available when using the currently loaded scene.) + // + // Arguments: + // pclient Pointer to client object + // scene_config_name Path to scene config file + // sim_config_path Path to directory containing files referenced by + // the scene config file delay_after_loading_sec Time period to sleep after + // loading the scene (seconds) sim_instance_idx Simulation instance + // number to append to scene ID (used for parallel cooperating simulations) + // + // Returns: + // (Return) Initialization status + ASC_DECL Status Initialize(std::shared_ptr& pclient, + const std::string& scene_config_name = "", + const std::string& sim_config_path = "sim_config/", + float delay_after_load_sec = 0, + int sim_instance_idx = -1); + + // Returns a voxel occupation grid for the specified volume. The returned + // boolean array is the 3D voxel occupancy map returned as a linear array. + // A true value indicates the voxel is occupied while false means the voxel + // is empty. The number of elements in the array is (nx * ny * nz) where: + // nx = floor(x_size / resolution) + // ny = floor(y_size / resolution) + // nz = floor(z_size / resolution) + // + // The entries in the array are for voxels in the order: + // (x0, y0, z0), (x0, y0, z1), ..., (x0, y0, znz-1), + // (x0, y1, z0), (x0, y1, z1), ..., (x0, y1, znz-1), + // ... + // (x0, yny-1, z0), (x0, yny-1, z1), ..., (x0, yny-1, znz-1), + // (x1, y0, z0), (x1, y0, z1), ..., (x1, y0, znz-1), + // ... + // (xnx-1, yny-1, z0), (xnx-1, yny-1, z1),..., (xnx-1, yny-1, znz-1) + // + // where (x0, y0, and z0) is the voxel at the minimum north, east, down + // position (far back, left, and up) while (xnx-1, ynx-1, znz-1) is at the + // maximum north, east, down position (far front, right, and down.) Thus, + // the voxel at position (i, j, k) is at index (i * ny * nz + j * nz + k) + // of the returned boolean array. + // + // + // Arguments: + // position Center of volume to inspect (orientation ignored) + // x_size Volume length along X-axis (meters) + // y_size Volume length along Y-axis (meters) + // z_size Volume length along Z-axis (meters) + // resolution Length of each edge of a voxel (meters) + // pbool_array_ret The buffer to get the boolean area indicating occupancy + // actors_to_ignore IDs of actors to ignore for occupancy + // write_file If true, write the voxel grid to file_path in binvox + // format file_path Path to output file if write_file is true + // + // Returns: + // (Return) Call status + // pbool_array_ret If call status is Status::OK, updated with the occupancy + // array + ASC_DECL Status + CreateVoxelGrid(const struct Pose& position, int x_size, int y_size, + int z_size, float resolution, BoolArray* pbool_array_ret, + const std::vector& actors_to_ignore = + std::vector(), + bool write_file = false, + std::string file_path = "./voxel_grid.binvox") const; + + // Return the scene configuration JSON loaded for this world (empty if + // Initialize() was not called with a scene config file name.) + ASC_DECL const json& GetConfiguration(void) const; + + // Return the names of the drones in the scene + ASC_DECL const VecStr& GetDrones(void) const; + + // Return the prefix for topics associated with this scene + ASC_DECL const char* GetParentTopic(void) const; + + // Simulation time and stepping + ASC_DECL Status GetSimClockType(std::string* pstr_clock_type_out) const; + ASC_DECL Status GetSimTime(std::int64_t* ptime_nanos_out) const; + ASC_DECL Status Pause(std::string* pstr_state_out = nullptr); + ASC_DECL Status Resume(std::string* pstr_state_out = nullptr); + ASC_DECL Status IsPaused(bool* pf_is_paused_out) const; + ASC_DECL Status ContinueForSimTime(std::int64_t delta_time_nanos, + bool wait_until_complete, + std::int64_t* ptime_nanos_out); + ASC_DECL Status ContinueUntilSimTime(std::int64_t target_time_nanos, + bool wait_until_complete, + std::int64_t* ptime_nanos_out); + ASC_DECL Status ContinueForNSteps(int n_steps, bool wait_until_complete, + std::int64_t* ptime_nanos_out); + ASC_DECL Status ContinueForSingleStep(bool wait_until_complete, + std::int64_t* ptime_nanos_out); + + // Environment + ASC_DECL Status SetSunlightIntensity(float intensity, bool* pf_success_out); + ASC_DECL Status GetSunlightIntensity(float* pintensity_out) const; + ASC_DECL Status SetCloudShadowStrength(float strength, bool* pf_success_out); + ASC_DECL Status GetCloudShadowStrength(float* pstrength_out) const; + ASC_DECL Status SetWindVelocity(float v_x, float v_y, float v_z, + bool* pf_success_out); + ASC_DECL Status GetWindVelocity(Vector3* pwind_velocity_out) const; + + // Scene objects and assets + ASC_DECL Status ListActors(VecStr* pvecstr_actor_names_out) const; + ASC_DECL Status ListObjects(const std::string& name_regex, + VecStr* pvecstr_object_names_out) const; + ASC_DECL Status ListAssets(const std::string& name_regex, + VecStr* pvecstr_asset_names_out) const; + ASC_DECL Status GetObjectPose(const std::string& object_name, + json* pjson_pose_out) const; + ASC_DECL Status GetObjectPoses(const VecStr& object_names, + json* pjson_poses_out) const; + ASC_DECL Status SetObjectPose(const std::string& object_name, + const json& json_pose, bool teleport, + bool* pf_success_out); + ASC_DECL Status GetObjectScale(const std::string& object_name, + std::vector* pvec_scale_out) const; + ASC_DECL Status SetObjectScale(const std::string& object_name, + const std::vector& scale, + bool* pf_success_out); + ASC_DECL Status SpawnObject(const std::string& object_name, + const std::string& asset_path, + const json& json_pose, + const std::vector& scale, + bool enable_physics, + std::string* pstr_spawned_name_out = nullptr); + ASC_DECL Status SpawnObjectFromFile( + const std::string& object_name, const std::string& file_format, + const VecB& asset_bytes, bool is_binary, const json& json_pose, + const std::vector& scale, bool enable_physics, + std::string* pstr_spawned_name_out = nullptr); + ASC_DECL Status SpawnObjectAtGeo(const std::string& object_name, + const std::string& asset_path, + float latitude, float longitude, + float altitude, + const std::vector& rotation, + const std::vector& scale, + bool enable_physics, + std::string* pstr_spawned_name_out = nullptr); + ASC_DECL Status SpawnObjectFromFileAtGeo( + const std::string& object_name, const std::string& file_format, + const VecB& asset_bytes, bool is_binary, float latitude, float longitude, + float altitude, const std::vector& rotation, + const std::vector& scale, bool enable_physics, + std::string* pstr_spawned_name_out = nullptr); + ASC_DECL Status DestroyObject(const std::string& object_name, + bool* pf_success_out); + ASC_DECL Status DestroyAllSpawnedObjects(bool* pf_success_out); + + // Weather, materials, lighting, segmentation + ASC_DECL Status EnableWeatherVisualEffects(bool* pf_success_out); + ASC_DECL Status DisableWeatherVisualEffects(bool* pf_success_out); + ASC_DECL Status ResetWeatherEffects(bool* pf_success_out); + ASC_DECL Status SetWeatherVisualEffectsParam(int param, float value, + bool* pf_success_out); + ASC_DECL Status GetWeatherVisualEffectsParam(json* pjson_params_out) const; + ASC_DECL Status SetObjectMaterial(const std::string& object_name, + const std::string& material_asset_path, + bool* pf_success_out); + ASC_DECL Status SetObjectTextureFromUrl(const std::string& object_name, + const std::string& url, + bool* pf_success_out); + ASC_DECL Status SetObjectTextureFromFile( + const std::string& object_name, const std::string& texture_file_path, + bool* pf_success_out); + ASC_DECL Status SetObjectTextureFromPackagedAsset( + const std::string& object_name, const std::string& texture_asset_path, + bool* pf_success_out); + ASC_DECL Status SwapObjectTexture(const std::string& tag, int tex_id, + bool* pf_success_out); + ASC_DECL Status SetLightObjectIntensity(const std::string& object_name, + float new_intensity, + bool* pf_success_out); + ASC_DECL Status SetLightObjectColor(const std::string& object_name, + const std::vector& color_rgb, + bool* pf_success_out); + ASC_DECL Status SetLightObjectRadius(const std::string& object_name, + float new_radius, bool* pf_success_out); + ASC_DECL Status SetTimeOfDay(bool enabled, const std::string& datetime, + bool is_dst, float clock_speed, + float update_interval, bool move_sun, + bool* pf_success_out); + ASC_DECL Status GetTimeOfDay(json* pjson_time_of_day_out) const; + ASC_DECL Status SetSunPositionFromDateTime(const std::string& date_time, + bool is_dst, + bool* pf_success_out); + ASC_DECL Status SwitchStreamingView(bool* pf_success_out); + ASC_DECL Status SetSegmentationIDByName(const std::string& mesh_name, + int segmentation_id, + bool is_name_regex, + bool use_owner_name, + bool* pf_success_out); + ASC_DECL Status GetSegmentationIDByName(const std::string& mesh_name, + bool use_owner_name, + int* psegmentation_id_out) const; + ASC_DECL Status GetSegmentationIDMap(json* pjson_segmentation_map_out) const; + + // Import a NED trajectory that can be assigned to robots or env actors. + ASC_DECL Status ImportNEDTrajectory( + const std::string& traj_name, const std::vector& time, + const std::vector& pose_x, const std::vector& pose_y, + const std::vector& pose_z, + const std::vector& pose_roll = std::vector(), + const std::vector& pose_pitch = std::vector(), + const std::vector& pose_yaw = std::vector(), + const std::vector& vel_lin_x = std::vector(), + const std::vector& vel_lin_y = std::vector(), + const std::vector& vel_lin_z = std::vector()); + + // + // Debug Plots + // + ASC_DECL Status FlushPersistentMarkers(void); + ASC_DECL Status PlotDebugArrows(const VecVector3& points_start, + const VecVector3& points_end, + const ColorRGBA& color_rgba, float thickness, + float arrow_size, float sec_duration, + bool is_persistent); + ASC_DECL Status PlotDebugDashedLine(const VecVector3& points, + const ColorRGBA& color_rgba, + float thickness, float sec_duration, + bool is_persistent); + ASC_DECL Status PlotDebugPoints(const VecVector3& points, + const ColorRGBA& color_rgba, float size, + float sec_duration, bool is_persistent); + ASC_DECL Status PlotDebugSolidLine(const VecVector3& points, + const ColorRGBA& color_rgba, + float thickness, float sec_duration, + bool is_persistent); + ASC_DECL Status PlotDebugStrings(const VecStr& strings, + const VecVector3& positions, float scale, + const ColorRGBA& color_rgba, + float sec_duration); + ASC_DECL Status PlotDebugTransforms(const VecPose& poses, float scale, + float thickness, float sec_duration, + bool is_persistent); + ASC_DECL Status PlotDebugTransformsWithNames( + const VecPose& poses, const VecStr& names, float tf_scale, + float tf_thickness, float text_scale, const ColorRGBA& text_color_rgba, + float sec_duration); + ASC_DECL Status SetTraceLine(const ColorRGBA& color_rgba, float thickness, + bool* pf_success_out); + ASC_DECL Status ToggleTrace(bool* pf_success_out); + ASC_DECL Status Get3DBoundingBox(const std::string& object_name, + int box_alignment, + json* pjson_bounding_box_out) const; + ASC_DECL Status ImportGeoTrajectory( + const std::string& traj_name, const std::vector& time, + const std::vector& latitudes, + const std::vector& longitudes, + const std::vector& altitudes, + const std::vector& roll = std::vector(), + const std::vector& pitch = std::vector(), + const std::vector& yaw = std::vector(), + const std::vector& vel_lin_x = std::vector(), + const std::vector& vel_lin_y = std::vector(), + const std::vector& vel_lin_z = std::vector()); + ASC_DECL Status GetSurfaceElevationAtPoint(float x, float y, + float* pelevation_out) const; + + // + // Interactive UI Methods + // + + // Return the 3D position of the first location hit by a ray projected + // along the local X-axis of the specified pose. + // + // Arguments: + // pose Ray is projected from this point at this orientation + // pvec3_out Buffer to get hit location + // + // Returns: + // (Return) Operation status + // pvec3_out If return status is Status::OK, contains the location + // where the ray first hit an object, or (NaN, NaN, Nan) + // if nothing was hit (NED coordinates) + ASC_DECL Status HitTest(const Pose& pose, Vector3* pvec3_out); + + protected: + class Impl; // World implementation + + protected: + std::shared_ptr pimpl_; // Pointer to implementation +}; // class World + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/AirSimClientDLL/pch.h b/client/cpp/ProjectAirsimClientLib/pch.h similarity index 87% rename from client/cpp/AirSimClientDLL/pch.h rename to client/cpp/ProjectAirsimClientLib/pch.h index 782e49f8..0745a0e1 100644 --- a/client/cpp/AirSimClientDLL/pch.h +++ b/client/cpp/ProjectAirsimClientLib/pch.h @@ -14,13 +14,14 @@ #define _CRT_SECURE_NO_WARNINGS #define _SILENCE_CXX17_CODECVT_HEADER_DEPRECATION_WARNING // is deprecated but there's no replacement yet (2/7/2023) -#define WINDOWS_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN #define NOMINMAX -#include "framework.h" -#include "resource.h" +#ifdef _WIN32 +#include +#endif -#include "AirSimClient.h" +#include "ProjectAirsimClient.h" #include "ASCDecl.h" #include "AsyncResult.h" #include "Client.h" @@ -41,9 +42,9 @@ #include "../../src/Topics.h" #include "../../src/Utils.h" -#include "AirSimMessage/jsonc.hpp" -#include -#include +#include "ProjectAirSimMessage/jsonc.hpp" +#include +#include #include "core_sim/math_utils.hpp" diff --git a/client/cpp/AirSimClientDLL/src/AsyncResult.cpp b/client/cpp/ProjectAirsimClientLib/src/AsyncResult.cpp similarity index 100% rename from client/cpp/AirSimClientDLL/src/AsyncResult.cpp rename to client/cpp/ProjectAirsimClientLib/src/AsyncResult.cpp diff --git a/client/cpp/AirSimClientDLL/src/AsyncResultInternal.cpp b/client/cpp/ProjectAirsimClientLib/src/AsyncResultInternal.cpp similarity index 99% rename from client/cpp/AirSimClientDLL/src/AsyncResultInternal.cpp rename to client/cpp/ProjectAirsimClientLib/src/AsyncResultInternal.cpp index 7f141d92..f6f9e117 100644 --- a/client/cpp/AirSimClientDLL/src/AsyncResultInternal.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/AsyncResultInternal.cpp @@ -5,7 +5,7 @@ #include "AsyncResultInternal.h" -#include "AirSimClient.h" +#include "ProjectAirsimClient.h" #include "pch.h" namespace microsoft { diff --git a/client/cpp/AirSimClientDLL/src/AsyncResultInternal.h b/client/cpp/ProjectAirsimClientLib/src/AsyncResultInternal.h similarity index 99% rename from client/cpp/AirSimClientDLL/src/AsyncResultInternal.h rename to client/cpp/ProjectAirsimClientLib/src/AsyncResultInternal.h index 58f662c2..5a710f83 100644 --- a/client/cpp/AirSimClientDLL/src/AsyncResultInternal.h +++ b/client/cpp/ProjectAirsimClientLib/src/AsyncResultInternal.h @@ -4,11 +4,12 @@ // MIT License. All rights reserved. #pragma once -#include +#include #include #include #include "AsyncResult.h" +#include "Message.h" #include "RefCounted.h" namespace microsoft { diff --git a/client/cpp/AirSimClientDLL/src/Client.cpp b/client/cpp/ProjectAirsimClientLib/src/Client.cpp similarity index 87% rename from client/cpp/AirSimClientDLL/src/Client.cpp rename to client/cpp/ProjectAirsimClientLib/src/Client.cpp index f87225c2..878a19e3 100644 --- a/client/cpp/AirSimClientDLL/src/Client.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/Client.cpp @@ -5,13 +5,13 @@ #include -#include -#include +#include +#include #include #include #include -#include "AirSimClient.h" +#include "ProjectAirsimClient.h" #include "AsyncResultInternal.h" #include "IMessageBuffer.h" #include "RefCounted.h" @@ -20,6 +20,38 @@ #include "json.hpp" #include "pch.h" +namespace { + +void PackJson(msgpack::packer* ppacker, + const nlohmann::json& value) { + if (value.is_null()) { + ppacker->pack_nil(); + } else if (value.is_boolean()) { + ppacker->pack(value.get()); + } else if (value.is_number_integer()) { + ppacker->pack(value.get()); + } else if (value.is_number_unsigned()) { + ppacker->pack(value.get()); + } else if (value.is_number_float()) { + ppacker->pack(value.get()); + } else if (value.is_string()) { + ppacker->pack(value.get()); + } else if (value.is_array()) { + ppacker->pack_array(value.size()); + for (const auto& item : value) PackJson(ppacker, item); + } else if (value.is_object()) { + ppacker->pack_map(value.size()); + for (auto it = value.begin(); it != value.end(); ++it) { + ppacker->pack(it.key()); + PackJson(ppacker, it.value()); + } + } else { + ppacker->pack_nil(); + } +} + +} // namespace + namespace microsoft { namespace projectairsim { namespace client { @@ -124,6 +156,9 @@ class Client::Impl { Status Request(const std::string& str_method, const nlohmann::json& json_parameters, client::Message* pmessage_response_out) noexcept; + Status SetInteractiveFeature(const std::string& str_feature_name, + bool f_enable, bool* pf_enabled_out); + Status GetBuildCommitHash(std::string* pstr_hash_out); TAsyncResult RequestAsync( const std::string& str_method, const nlohmann::json& json_parameters, FnResponseCallback fnresponse_callback = nullptr, @@ -134,6 +169,7 @@ class Client::Impl { const std::string& str_method, const nlohmann::json& json_parameters = json(), FnResponseCallback fnresponse_callback = nullptr) noexcept; + Status Publish(const char* sz_topic_name, const json& json_message); Status Subscribe(const char* sz_topic_name, TopicCallback topic_callback); Status Unsubscribe(const char* sz_topic_name); Status Unsubscribe(const std::vector& vec_topic_name, @@ -318,6 +354,17 @@ ASC_DECL Status Client::Request( pmessage_response_out)); } +ASC_DECL Status Client::SetInteractiveFeature(const std::string& str_feature_name, + bool f_enable, + bool* pf_enabled_out) { + return (pclient_impl_->SetInteractiveFeature(str_feature_name, f_enable, + pf_enabled_out)); +} + +ASC_DECL Status Client::GetBuildCommitHash(std::string* pstr_hash_out) { + return (pclient_impl_->GetBuildCommitHash(pstr_hash_out)); +} + ASC_DECL TAsyncResult Client::RequestAsync( const std::string& str_method, const nlohmann::json& json_parameters, FnResponseCallback fnresponse_callback) noexcept { @@ -332,6 +379,11 @@ Client::RequestLoadScene(const std::string& str_scene_config, pclient_impl_->RequestLoadScene(str_scene_config, pmessage_response_out)); } +ASC_DECL Status Client::Publish(const char* sz_topic_name, + const nlohmann::json& json_message) { + return (pclient_impl_->Publish(sz_topic_name, json_message)); +} + ASC_DECL Status Client::Subscribe(const char* sz_topic_name, TopicCallback topic_callback) { return (pclient_impl_->Subscribe(sz_topic_name, topic_callback)); @@ -590,6 +642,63 @@ Status Client::Impl::RequestLoadScene(const std::string& str_scene_config, return (status); } +Status Client::Impl::SetInteractiveFeature(const std::string& str_feature_name, + bool f_enable, + bool* pf_enabled_out) { + Status status; + Message message_response; + json json_params = { + {"feature_id", str_feature_name}, + {"enable", f_enable}, + }; + + if ((status = Request("/Sim/SetInteractiveFeature", json_params, + &message_response)) == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() == 0) + *pf_enabled_out = response_message.GetResult().is_boolean() && + static_cast(response_message.GetResult()); + else { + status = Status::RejectedByServer; + + log.ErrorF( + "Server method \"/Sim/SetInteractiveFeature\" failed: error %d: %s", + response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status Client::Impl::GetBuildCommitHash(std::string* pstr_hash_out) { + Status status; + Message message_response; + + if ((status = Request("/Sim/GetBuildCommitHash", json(), + &message_response)) == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() == 0) + *pstr_hash_out = static_cast(response_message.GetResult()); + else { + status = Status::RejectedByServer; + + log.ErrorF( + "Server method \"/Sim/GetBuildCommitHash\" failed: error %d: %s", + response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + TAsyncResult Client::Impl::RequestPriorityAsync( const std::string& str_method, const nlohmann::json& json_parameters, FnResponseCallback fnresponse_callback) noexcept { @@ -786,6 +895,31 @@ Status Client::Impl::SendRequest( return (status); } +Status Client::Impl::Publish(const char* sz_topic_name, + const json& json_message) { + if (!is_connected_) return (Status::NotConnected); + if (sz_topic_name == nullptr) return (Status::Failed); + + try { + msgpack::sbuffer body_buffer; + msgpack::sbuffer frame_buffer; + const int kMessageFrameType = 2; + msgpack::packer body_packer(body_buffer); + + PackJson(&body_packer, json_message); + std::tuple frame( + kMessageFrameType, sz_topic_name, + std::string(body_buffer.data(), body_buffer.size())); + msgpack::pack(frame_buffer, frame); + + return (pnng_server_channel_topics_->Send( + reinterpret_cast(frame_buffer.data()), + frame_buffer.size())); + } catch (...) { + return (Status::Failed); + } +} + Status Client::Impl::Subscribe(const char* sz_topic_name, TopicCallback topic_callback) { Status status = Status::OK; diff --git a/client/cpp/ProjectAirsimClientLib/src/Drone.cpp b/client/cpp/ProjectAirsimClientLib/src/Drone.cpp new file mode 100644 index 00000000..0c0a5735 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/src/Drone.cpp @@ -0,0 +1,1676 @@ +// Copyright (C) Microsoft Corporation. +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include "Drone.h" + +#include +#include +#include +#include + +#include "ProjectAirsimClient.h" +#include "ProjectAirSimMessage/response_message.hpp" +#include "AsyncResultInternal.h" +#include "Common.h" +#include "JSONUtils.h" +#include "pch.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class Drone::Impl { + public: + Impl(void) noexcept; + ~Impl(); + + Status Arm(bool* pf_is_armed_out); + Status CanArm(bool* pf_can_be_armed_out) const; + Status CancelLastTask(bool* pf_task_is_canceled_out); + Status DisableAPIControl(bool* pf_is_disabled_out); + Status Disarm(bool* pf_is_disarmed_out); + Status EnableAPIControl(bool* pf_is_enabled_out); + Status GetLandedState(LandedState* planded_state_out) const; + Status GetReadyState(ReadyState* pready_state_out) const; + Status GetCameraRay(const std::string& str_camera_name, ImageType image_type, + int x, int y, Pose* ppose_out); + Status GetIMUData(const std::string& str_sensor_name, json* pjson_out) const; + Status GetGPSData(const std::string& str_sensor_name, json* pjson_out) const; + Status GetAirspeedData(const std::string& str_sensor_name, + json* pjson_out) const; + Status GetBarometerData(const std::string& str_sensor_name, + json* pjson_out) const; + Status GetMagnetometerData(const std::string& str_sensor_name, + json* pjson_out) const; + Status GetLidarData(const std::string& str_sensor_name, + json* pjson_out) const; + Status GetRadarDetections(const std::string& str_sensor_name, + json* pjson_out) const; + Status GetRadarTracks(const std::string& str_sensor_name, + json* pjson_out) const; + Status GetBatteryState(json* pjson_out) const; + Status SetBatteryRemaining(float desired_battery_remaining, + bool* pf_is_set_out); + Status GetBatteryDrainRate(float* pf_drain_rate_out) const; + Status SetBatteryDrainRate(float desired_drain_rate, bool* pf_is_set_out); + Status SetBatteryHealthStatus(bool is_desired_state_healthy, + bool* pf_is_set_out); + Status GetImages(const std::string& str_camera_id, + const std::vector& veci_image_type_id, + json* pjson_out) const; + Status CameraLookAtObject(const std::string& str_camera_id, + const std::string& str_object_name, + bool wait_for_pose_update, bool* pf_success_out); + Status CameraDrawFrustum(const std::string& str_camera_id, bool to_enable, + ImageType image_type, bool* pf_success_out); + Status SetCameraPose(const std::string& str_camera_id, const json& json_pose, + bool wait_for_pose_update, bool* pf_success_out); + Status ResetCameraPose(const std::string& str_camera_id, + bool wait_for_pose_update, bool* pf_success_out); + Status SetFocalLength(const std::string& str_camera_id, int image_type_id, + float focal_length, bool* pf_success_out); + Status SetDepthOfFieldTransitionThreshold(const std::string& str_camera_id, + int image_type_id, + float transition_threshold, + bool* pf_success_out); + Status SetDepthOfFieldFocalRegion(const std::string& str_camera_id, + int image_type_id, + float max_focal_distance, + bool* pf_success_out); + Status SetChromaticAberrationIntensity(const std::string& str_camera_id, + int image_type_id, float intensity, + bool* pf_success_out); + Status SetFieldOfView(const std::string& str_camera_id, int image_type_id, + float field_of_view, bool* pf_success_out); + Status GetGroundTruthPose(Transform* ptransform_out); + Status GetGroundTruthGeoLocation(GeoPosition* pgeo_position_out); + Status GetEstimatedGeoLocation(GeoPosition* pgeo_position_out); + Status GetGroundTruthKinematics(json* pjson_out) const; + Status GetEstimatedKinematics(json* pjson_out) const; + Status SetGroundTruthKinematics(const json& json_kinematics, + bool* pf_success_out); + size_t GetRobotInfoCount(void) const; + const char* GetRobotInfoName(int iname) const; + const char* GetRobotTopic(const std::string& str_info_name) const; + size_t GetSensorCount(void) const; + size_t GetSensorInfoCount(const std::string& str_sensor_name) const; + const char* GetSensorInfoName(const std::string& str_sensor_name, + int iinfo) const; + const char* GetSensorName(int iname) const; + const char* GetSensorTopic(const std::string& str_sensor_name, + const std::string& str_info_name) const; + AsyncResult GoHomeAsync(float sec_timeout, float velocity, + FnResponseCallback fnresponse_callback); + AsyncResult HoverAsync(FnResponseCallback fnresponse_callback); + AsyncResult RequestControlAsync(FnResponseCallback fnresponse_callback); + AsyncResult SetMissionModeAsync(FnResponseCallback fnresponse_callback); + AsyncResult SetVTOLModeAsync(VTOLMode vtol_mode, + FnResponseCallback fnresponse_callback); + Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& str_drone_name); + Status IsAPIControlEnabled(bool* pf_is_enabled_out) const; + AsyncResult LandAsync(float sec_timeout, + FnResponseCallback fnresponse_callback); + AsyncResult MoveByVelocityAsync(float v_north, float v_east, float v_down, + float sec_duration, + YawControlMode yaw_control_mode, + bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback); + ASC_DECL AsyncResult MoveByVelocityZAsync( + float v_north, float v_east, float z, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback); + ASC_DECL AsyncResult MoveByVelocityBodyFrameAsync( + float v_forward, float v_right, float v_down, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback); + ASC_DECL AsyncResult MoveByVelocityBodyFrameZAsync( + float v_forward, float v_right, float z, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback); + ASC_DECL AsyncResult MoveByHeadingAsync(float heading, float speed, + float v_down, float sec_duration, + float heading_margin, float yaw_rate, + float timeout_sec, + FnResponseCallback + fnresponse_callback); + ASC_DECL AsyncResult MoveOnPathAsync(const VecVector3& path, float velocity, + float timeout_sec, + YawControlMode yaw_control_mode, + bool yaw_is_rate, float yaw, + float lookahead, + float adaptive_lookahead, + FnResponseCallback fnresponse_callback); + ASC_DECL AsyncResult MoveToPositionAsync( + float north, float east, float down, float velocity, float timeout_sec, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback); + ASC_DECL AsyncResult RotateByYawRateAsync( + float yaw_rate, float sec_duration, + FnResponseCallback fnresponse_callback); + ASC_DECL AsyncResult RotateToYawAsync(float yaw, float timeout_sec, + float margin, float yaw_rate, + FnResponseCallback fnresponse_callback); + Status SetPose(const Transform& transform, bool reset_kinematics); + Status SetControlSignals(const json& json_control_signal_map, + bool* pf_success_out); + Status UpdateActuatorFaultState(const std::string& actuator_id, + bool fault_configured, + bool* pf_success_out); + Status SetExternalForce(const std::vector& ext_force, + bool* pf_success_out); + AsyncResult TakeoffAsync(float sec_timeout, + FnResponseCallback fnresponse_callback); + + protected: + // Mapping from a string to another + typedef std::unordered_map UMStrStr; + + // Mapping from a string to a string-to-string map + typedef std::unordered_map UMStrUMStr; + + // Mapping from a string to vector of strings + typedef std::unordered_map> UMStrVecStr; + + protected: + void LogTopics(void) const; + void SetRobotInfoTopics(void); + void SetSensorTopics(World* pworld); + void SetTopics(void); + Status RequestDrone(const std::string str_method, + const json& json_params = json()) const; + Status RequestDrone(const std::string str_method, bool* pf_out) const; + Status RequestDrone(const std::string str_method, json* pjson_out) const; + Status RequestDrone(const std::string str_method, const json& json_params, + bool* pf_out) const; + Status RequestDrone(const std::string str_method, const json& json_params, + json* pjson_out) const; + AsyncResult RequestDroneAsync(const std::string str_method, + const json& json_params, + FnResponseCallback fnresponse_callback) const; + Status RequestMethod(const std::string& str_method_path, + const json& json_params, json* pjson_out) const; + Status RequestMethod(const std::string& str_method_path, + const json& json_params, bool* pf_out) const; + Status RequestMethod(const std::string& str_method_path, + const json& json_params, float* pf_out) const; + Status RequestMethod(const std::string& str_method_path, + const json& json_params = json()) const; + Status RequestSensorData(const std::string& str_sensor_name, + const std::string& str_sensor_info_name, + json* pjson_out) const; + + protected: + std::shared_ptr pclient_; // Pointer to client connection object + std::shared_ptr pworld_; // Pointer to scene object + std::string str_drone_name_; // Name of the drone + std::string str_parent_topic_; // Topic root of the drone + std::string str_sensor_topic_; // Topic root of the sensors + std::string str_world_parent_topic_; // Topic root of the scene + UMStrStr umrobot_name_topic_; // Mapping from robot info name to topic name + UMStrUMStr umsensor_name_uminfo_topic_; // Mapping from sensor name to + // mapping from info to topic name + UMStrVecStr umsensor_name_vecinfo_; // Mapping from sensor name to list of + // sensor info names + VecStr vecstr_robot_info_name_; // List of robot info names + VecStr vecstr_sensor_name_; // List of sensor names +}; // class Drone::Impl + +ASC_DECL const float Drone::kNoTimeout = std::numeric_limits::max(); + +ASC_DECL Drone::Drone(void) noexcept : pimpl_(new Impl()) {} + +ASC_DECL Drone::~Drone() {} + +Status Drone::Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& str_drone_name) { + RETURN_CATCH_STATUS(pimpl_->Initialize(pclient, pworld, str_drone_name)); +} + +ASC_DECL Status Drone::Arm(bool* pf_is_armed_out) { + RETURN_CATCH_STATUS(pimpl_->Arm(pf_is_armed_out)); +} + +ASC_DECL Status Drone::Disarm(bool* pf_is_disarmed_out) { + RETURN_CATCH_STATUS(pimpl_->Disarm(pf_is_disarmed_out)); +} + +ASC_DECL Status Drone::CanArm(bool* pf_can_be_armed_out) const { + RETURN_CATCH_STATUS(pimpl_->CanArm(pf_can_be_armed_out)); +} + +ASC_DECL Status Drone::GetReadyState(ReadyState* pready_state_out) const { + RETURN_CATCH_STATUS(pimpl_->GetReadyState(pready_state_out)); +} + +ASC_DECL Status Drone::GetLandedState(LandedState* planded_state_out) const { + RETURN_CATCH_STATUS(pimpl_->GetLandedState(planded_state_out)); +} + +ASC_DECL Status Drone::CancelLastTask(bool* pf_task_is_canceled_out) { + RETURN_CATCH_STATUS(pimpl_->CancelLastTask(pf_task_is_canceled_out)); +} + +ASC_DECL Status Drone::DisableAPIControl(bool* pf_is_disabled_out) { + RETURN_CATCH_STATUS(pimpl_->DisableAPIControl(pf_is_disabled_out)); +} + +ASC_DECL Status Drone::EnableAPIControl(bool* pf_is_enabled_out) { + RETURN_CATCH_STATUS(pimpl_->EnableAPIControl(pf_is_enabled_out)); +} + +ASC_DECL Status Drone::GetCameraRay(const std::string& str_camera_name, + ImageType image_type, int x, int y, + Pose* ppose_out) { + RETURN_CATCH_STATUS( + pimpl_->GetCameraRay(str_camera_name, image_type, x, y, ppose_out)); +} + +ASC_DECL Status Drone::GetIMUData(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetIMUData(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetGPSData(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetGPSData(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetAirspeedData(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetAirspeedData(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetBarometerData(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetBarometerData(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetMagnetometerData(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetMagnetometerData(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetLidarData(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetLidarData(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetRadarDetections(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetRadarDetections(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetRadarTracks(const std::string& str_sensor_name, + json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetRadarTracks(str_sensor_name, pjson_out)); +} + +ASC_DECL Status Drone::GetBatteryState(json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetBatteryState(pjson_out)); +} + +ASC_DECL Status Drone::SetBatteryRemaining(float desired_battery_remaining, + bool* pf_is_set_out) { + RETURN_CATCH_STATUS( + pimpl_->SetBatteryRemaining(desired_battery_remaining, pf_is_set_out)); +} + +ASC_DECL Status Drone::GetBatteryDrainRate(float* pf_drain_rate_out) const { + RETURN_CATCH_STATUS(pimpl_->GetBatteryDrainRate(pf_drain_rate_out)); +} + +ASC_DECL Status Drone::SetBatteryDrainRate(float desired_drain_rate, + bool* pf_is_set_out) { + RETURN_CATCH_STATUS( + pimpl_->SetBatteryDrainRate(desired_drain_rate, pf_is_set_out)); +} + +ASC_DECL Status Drone::SetBatteryHealthStatus(bool is_desired_state_healthy, + bool* pf_is_set_out) { + RETURN_CATCH_STATUS( + pimpl_->SetBatteryHealthStatus(is_desired_state_healthy, pf_is_set_out)); +} + +ASC_DECL Status Drone::GetImages(const std::string& str_camera_id, + const std::vector& veci_image_type_id, + json* pjson_out) const { + RETURN_CATCH_STATUS( + pimpl_->GetImages(str_camera_id, veci_image_type_id, pjson_out)); +} + +ASC_DECL Status Drone::CameraLookAtObject(const std::string& str_camera_id, + const std::string& str_object_name, + bool wait_for_pose_update, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->CameraLookAtObject( + str_camera_id, str_object_name, wait_for_pose_update, pf_success_out)); +} + +ASC_DECL Status Drone::CameraDrawFrustum(const std::string& str_camera_id, + bool to_enable, + ImageType image_type, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->CameraDrawFrustum(str_camera_id, to_enable, + image_type, pf_success_out)); +} + +ASC_DECL Status Drone::SetCameraPose(const std::string& str_camera_id, + const json& json_pose, + bool wait_for_pose_update, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->SetCameraPose(str_camera_id, json_pose, + wait_for_pose_update, + pf_success_out)); +} + +ASC_DECL Status Drone::ResetCameraPose(const std::string& str_camera_id, + bool wait_for_pose_update, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->ResetCameraPose(str_camera_id, + wait_for_pose_update, + pf_success_out)); +} + +ASC_DECL Status Drone::SetFocalLength(const std::string& str_camera_id, + int image_type_id, float focal_length, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->SetFocalLength(str_camera_id, image_type_id, + focal_length, pf_success_out)); +} + +ASC_DECL Status Drone::SetDepthOfFieldTransitionThreshold( + const std::string& str_camera_id, int image_type_id, + float transition_threshold, bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->SetDepthOfFieldTransitionThreshold( + str_camera_id, image_type_id, transition_threshold, pf_success_out)); +} + +ASC_DECL Status Drone::SetDepthOfFieldFocalRegion( + const std::string& str_camera_id, int image_type_id, + float max_focal_distance, bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->SetDepthOfFieldFocalRegion( + str_camera_id, image_type_id, max_focal_distance, pf_success_out)); +} + +ASC_DECL Status Drone::SetChromaticAberrationIntensity( + const std::string& str_camera_id, int image_type_id, float intensity, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->SetChromaticAberrationIntensity( + str_camera_id, image_type_id, intensity, pf_success_out)); +} + +ASC_DECL Status Drone::SetFieldOfView(const std::string& str_camera_id, + int image_type_id, float field_of_view, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->SetFieldOfView(str_camera_id, image_type_id, + field_of_view, pf_success_out)); +} + +ASC_DECL Status Drone::GetGroundTruthPose(Transform* ptransform_out) { + RETURN_CATCH_STATUS(pimpl_->GetGroundTruthPose(ptransform_out)); +} + +ASC_DECL Status Drone::GetGroundTruthGeoLocation( + GeoPosition* pgeo_position_out) { + RETURN_CATCH_STATUS(pimpl_->GetGroundTruthGeoLocation(pgeo_position_out)); +} + +ASC_DECL Status Drone::GetEstimatedGeoLocation(GeoPosition* pgeo_position_out) { + RETURN_CATCH_STATUS(pimpl_->GetEstimatedGeoLocation(pgeo_position_out)); +} + +ASC_DECL Status Drone::GetGroundTruthKinematics(json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetGroundTruthKinematics(pjson_out)); +} + +ASC_DECL Status Drone::GetEstimatedKinematics(json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetEstimatedKinematics(pjson_out)); +} + +ASC_DECL Status Drone::SetGroundTruthKinematics(const json& json_kinematics, + bool* pf_success_out) { + RETURN_CATCH_STATUS( + pimpl_->SetGroundTruthKinematics(json_kinematics, pf_success_out)); +} + +ASC_DECL Status Drone::SetControlSignals(const json& json_control_signal_map, + bool* pf_success_out) { + RETURN_CATCH_STATUS( + pimpl_->SetControlSignals(json_control_signal_map, pf_success_out)); +} + +ASC_DECL Status Drone::UpdateActuatorFaultState( + const std::string& actuator_id, bool fault_configured, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->UpdateActuatorFaultState( + actuator_id, fault_configured, pf_success_out)); +} + +ASC_DECL Status Drone::SetExternalForce(const std::vector& ext_force, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->SetExternalForce(ext_force, pf_success_out)); +} + +ASC_DECL size_t Drone::GetRobotInfoCount(void) const { + return (pimpl_->GetRobotInfoCount()); +} + +ASC_DECL const char* Drone::GetRobotInfoName(int iname) const { + return (pimpl_->GetRobotInfoName(iname)); +} + +ASC_DECL const char* Drone::GetRobotTopic( + const std::string& str_info_name) const { + return (pimpl_->GetRobotTopic(str_info_name)); +} + +ASC_DECL size_t Drone::GetSensorCount(void) const { + return (pimpl_->GetSensorCount()); +} + +ASC_DECL size_t +Drone::GetSensorInfoCount(const std::string& str_sensor_name) const { + return (pimpl_->GetSensorInfoCount(str_sensor_name)); +} + +ASC_DECL const char* Drone::GetSensorInfoName( + const std::string& str_sensor_name, int iinfo) const { + return (pimpl_->GetSensorInfoName(str_sensor_name, iinfo)); +} + +ASC_DECL const char* Drone::GetSensorName(int iname) const { + return (pimpl_->GetSensorName(iname)); +} + +ASC_DECL const char* Drone::GetSensorTopic( + const std::string& str_sensor_name, + const std::string& str_info_name) const { + return (pimpl_->GetSensorTopic(str_sensor_name, str_info_name)); +} + +ASC_DECL AsyncResult Drone::GoHomeAsync( + float sec_timeout, float velocity, FnResponseCallback fnresponse_callback) { + return (pimpl_->GoHomeAsync(sec_timeout, velocity, fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::HoverAsync(FnResponseCallback fnresponse_callback) { + return (pimpl_->HoverAsync(fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::RequestControlAsync( + FnResponseCallback fnresponse_callback) { + return (pimpl_->RequestControlAsync(fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::SetMissionModeAsync( + FnResponseCallback fnresponse_callback) { + return (pimpl_->SetMissionModeAsync(fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::SetVTOLModeAsync( + VTOLMode vtol_mode, FnResponseCallback fnresponse_callback) { + return (pimpl_->SetVTOLModeAsync(vtol_mode, fnresponse_callback)); +} + +ASC_DECL Status Drone::IsAPIControlEnabled(bool* pf_is_enabled_out) const { + RETURN_CATCH_STATUS(pimpl_->IsAPIControlEnabled(pf_is_enabled_out)); +} + +ASC_DECL AsyncResult Drone::LandAsync(float sec_timeout, + FnResponseCallback fnresponse_callback) { + return (pimpl_->LandAsync(sec_timeout, fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::MoveByVelocityAsync( + float v_north, float v_east, float v_down, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveByVelocityAsync(v_north, v_east, v_down, sec_duration, + yaw_control_mode, yaw_is_rate, yaw, + fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::MoveByVelocityZAsync( + float v_north, float v_east, float z, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveByVelocityZAsync(v_north, v_east, z, sec_duration, + yaw_control_mode, yaw_is_rate, yaw, + fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::MoveByVelocityBodyFrameAsync( + float v_forward, float v_right, float v_down, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveByVelocityBodyFrameAsync( + v_forward, v_right, v_down, sec_duration, yaw_control_mode, yaw_is_rate, + yaw, fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::MoveByVelocityBodyFrameZAsync( + float v_forward, float v_right, float z, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveByVelocityBodyFrameZAsync( + v_forward, v_right, z, sec_duration, yaw_control_mode, yaw_is_rate, yaw, + fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::MoveByHeadingAsync( + float heading, float speed, float v_down, float sec_duration, + float heading_margin, float yaw_rate, float timeout_sec, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveByHeadingAsync(heading, speed, v_down, sec_duration, + heading_margin, yaw_rate, timeout_sec, + fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::MoveOnPathAsync( + const VecVector3& path, float velocity, float timeout_sec, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveOnPathAsync(path, velocity, timeout_sec, yaw_control_mode, + yaw_is_rate, yaw, lookahead, + adaptive_lookahead, fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::MoveToPositionAsync( + float north, float east, float down, float velocity, float timeout_sec, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveToPositionAsync( + north, east, down, velocity, timeout_sec, yaw_control_mode, yaw_is_rate, + yaw, lookahead, adaptive_lookahead, fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::RotateToYawAsync(float yaw, float timeout_sec, + float margin, float yaw_rate, + FnResponseCallback + fnresponse_callback) { + return (pimpl_->RotateToYawAsync(yaw, timeout_sec, margin, yaw_rate, + fnresponse_callback)); +} + +ASC_DECL AsyncResult Drone::RotateByYawRateAsync( + float yaw_rate, float sec_duration, + FnResponseCallback fnresponse_callback) { + return (pimpl_->RotateByYawRateAsync(yaw_rate, sec_duration, + fnresponse_callback)); +} + +ASC_DECL Status Drone::SetPose(const Transform& transform, + bool reset_kinematics) { + RETURN_CATCH_STATUS(pimpl_->SetPose(transform, reset_kinematics)); +} + +ASC_DECL AsyncResult +Drone::TakeoffAsync(float sec_timeout, FnResponseCallback fnresponse_callback) { + return (pimpl_->TakeoffAsync(sec_timeout, fnresponse_callback)); +} + +Drone::Impl::Impl() noexcept + : pclient_(), + pworld_(), + str_drone_name_(), + str_parent_topic_(), + str_sensor_topic_(), + str_world_parent_topic_(), + umrobot_name_topic_(), + umsensor_name_uminfo_topic_(), + umsensor_name_vecinfo_(), + vecstr_robot_info_name_(), + vecstr_sensor_name_() {} + +Drone::Impl::~Impl() {} + +Status Drone::Impl::Arm(bool* pf_is_armed_out) { + return (RequestDrone("Arm", pf_is_armed_out)); +} + +Status Drone::Impl::CanArm(bool* pf_can_be_armed_out) const { + return (RequestDrone("CanArm", pf_can_be_armed_out)); +} + +Status Drone::Impl::GetReadyState(ReadyState* pready_state_out) const { + Status status; + json json_response; + + if ((status = RequestDrone("GetReadyState", &json_response)) == Status::OK) + *pready_state_out = json_response; + + return (status); +} + +Status Drone::Impl::GetLandedState(LandedState* planded_state_out) const { + Status status; + json json_response; + + if ((status = RequestDrone("GetLandedState", &json_response)) == Status::OK) + *planded_state_out = static_cast(json_response.get()); + + return (status); +} + +Status Drone::Impl::CancelLastTask(bool* pf_task_is_canceled_out) { + TAsyncResult ar; + Status status; + + ar = pclient_->RequestPriorityAsync(str_parent_topic_ + "/CancelLastTask", json(), nullptr); + if ((status = ar.Wait()) == Status::OK) { + const Message& message = ar.GetResult(); + ResponseMessage response_message; + + response_message.Deserialize(message); + if (response_message.GetErrorCode() == 0) + *pf_task_is_canceled_out = response_message.GetResult().is_boolean() && static_cast(response_message.GetResult()); + else + { + status = Status::RejectedByServer; + + log.ErrorF( + "Server method \"CancelLastTask\" failed: error %d: %s", + response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status Drone::Impl::DisableAPIControl(bool* pf_is_disabled_out) { + return (RequestDrone("DisableApiControl", pf_is_disabled_out)); +} + +Status Drone::Impl::Disarm(bool* pf_is_disarmed_out) { + return (RequestDrone("Disarm", pf_is_disarmed_out)); +} + +Status Drone::Impl::EnableAPIControl(bool* pf_is_enabled_out) { + return (RequestDrone("EnableApiControl", pf_is_enabled_out)); +} + +Status Drone::Impl::GetCameraRay(const std::string& str_camera_name, + ImageType image_type, int x, int y, + Pose* ppose_out) { + Status status; + json json_params = {{"camera_id", str_camera_name}, + {"image_type", (int)image_type}, + {"x", x}, + {"y", y}}; + json json_response; + + if ((status = RequestDrone("GetCameraRay", json_params, &json_response)) == + Status::OK) + *ppose_out = json_response; + + return (status); +} + +Status Drone::Impl::GetIMUData(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "imu_kinematics", pjson_out)); +} + +Status Drone::Impl::GetGPSData(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "gps", pjson_out)); +} + +Status Drone::Impl::GetAirspeedData(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "airspeed", pjson_out)); +} + +Status Drone::Impl::GetBarometerData(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "barometer", pjson_out)); +} + +Status Drone::Impl::GetMagnetometerData(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "magnetometer", pjson_out)); +} + +Status Drone::Impl::GetLidarData(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "lidar", pjson_out)); +} + +Status Drone::Impl::GetRadarDetections(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "radar_detections", pjson_out)); +} + +Status Drone::Impl::GetRadarTracks(const std::string& str_sensor_name, + json* pjson_out) const { + return (RequestSensorData(str_sensor_name, "radar_tracks", pjson_out)); +} + +Status Drone::Impl::GetBatteryState(json* pjson_out) const { + return (RequestMethod(str_sensor_topic_ + "/Battery/GetBatteryStatus", json(), + pjson_out)); +} + +Status Drone::Impl::SetBatteryRemaining(float desired_battery_remaining, + bool* pf_is_set_out) { + json json_params = { + {"desired_battery_remaining", desired_battery_remaining}, + }; + + return (RequestMethod(str_sensor_topic_ + "/Battery/SetBatteryRemaining", + json_params, pf_is_set_out)); +} + +Status Drone::Impl::GetBatteryDrainRate(float* pf_drain_rate_out) const { + json json_response; + Status status; + + status = RequestMethod(str_sensor_topic_ + "/Battery/GetBatteryDrainRate", + json(), &json_response); + if (status == Status::OK) *pf_drain_rate_out = json_response.get(); + + return (status); +} + +Status Drone::Impl::SetBatteryDrainRate(float desired_drain_rate, + bool* pf_is_set_out) { + json json_params = { + {"desired_drain_rate", desired_drain_rate}, + }; + + return (RequestMethod(str_sensor_topic_ + "/Battery/SetBatteryDrainRate", + json_params, pf_is_set_out)); +} + +Status Drone::Impl::SetBatteryHealthStatus(bool is_desired_state_healthy, + bool* pf_is_set_out) { + json json_params = { + {"battery_health_indicator", is_desired_state_healthy}, + }; + + return ( + RequestMethod(str_sensor_topic_ + "/Battery/SetBatteryHealthStatus", + json_params, pf_is_set_out)); +} + +Status Drone::Impl::GetImages(const std::string& str_camera_id, + const std::vector& veci_image_type_id, + json* pjson_out) const { + json json_params = { + {"image_type_ids", veci_image_type_id}, + }; + + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + "/GetImages", + json_params, pjson_out)); +} + +Status Drone::Impl::CameraLookAtObject(const std::string& str_camera_id, + const std::string& str_object_name, + bool wait_for_pose_update, + bool* pf_success_out) { + json json_params = { + {"object_name", str_object_name}, + {"wait_for_pose_update", wait_for_pose_update}, + }; + + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/LookAtObject", + json_params, pf_success_out)); +} + +Status Drone::Impl::CameraDrawFrustum(const std::string& str_camera_id, + bool to_enable, ImageType image_type, + bool* pf_success_out) { + json json_params = { + {"image_type", static_cast(image_type)}, + {"to_enable", to_enable}, + }; + + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/DrawFrustum", + json_params, pf_success_out)); +} + +Status Drone::Impl::SetCameraPose(const std::string& str_camera_id, + const json& json_pose, + bool wait_for_pose_update, + bool* pf_success_out) { + return (RequestMethod( + str_sensor_topic_ + "/" + str_camera_id + "/SetPose", + json{{"pose", json_pose}, {"wait_for_pose_update", wait_for_pose_update}}, + pf_success_out)); +} + +Status Drone::Impl::ResetCameraPose(const std::string& str_camera_id, + bool wait_for_pose_update, + bool* pf_success_out) { + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/ResetCameraPose", + json{{"wait_for_pose_update", wait_for_pose_update}}, + pf_success_out)); +} + +Status Drone::Impl::SetFocalLength(const std::string& str_camera_id, + int image_type_id, float focal_length, + bool* pf_success_out) { + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/SetFocalLength", + json{{"image_type_id", image_type_id}, + {"focal_length", focal_length}}, + pf_success_out)); +} + +Status Drone::Impl::SetDepthOfFieldTransitionThreshold( + const std::string& str_camera_id, int image_type_id, + float transition_threshold, bool* pf_success_out) { + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/SetDepthOfFieldTransitionRegion", + json{{"image_type_id", image_type_id}, + {"transition_threshold", transition_threshold}}, + pf_success_out)); +} + +Status Drone::Impl::SetDepthOfFieldFocalRegion( + const std::string& str_camera_id, int image_type_id, + float max_focal_distance, bool* pf_success_out) { + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/SetDepthOfFieldFocalRegion", + json{{"image_type_id", image_type_id}, + {"focal_length", max_focal_distance}}, + pf_success_out)); +} + +Status Drone::Impl::SetChromaticAberrationIntensity( + const std::string& str_camera_id, int image_type_id, float intensity, + bool* pf_success_out) { + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/SetChromaticAberrationIntensity", + json{{"image_type_id", image_type_id}, + {"intensity", intensity}}, + pf_success_out)); +} + +Status Drone::Impl::SetFieldOfView(const std::string& str_camera_id, + int image_type_id, float field_of_view, + bool* pf_success_out) { + return (RequestMethod(str_sensor_topic_ + "/" + str_camera_id + + "/SetFieldOfView", + json{{"image_type_id", image_type_id}, + {"field_of_view", field_of_view}}, + pf_success_out)); +} + +Status Drone::Impl::GetGroundTruthPose(Transform* ptransform_out) { + Status status; + json json_response; + + if ((status = RequestDrone("GetGroundTruthPose", &json_response)) == + Status::OK) + *ptransform_out = json_response; + + return (status); +} + +Status Drone::Impl::GetGroundTruthGeoLocation( + GeoPosition* pgeo_position_out) { + Status status; + json json_response; + + if ((status = RequestDrone("GetGroundTruthGeoLocation", &json_response)) == + Status::OK) + *pgeo_position_out = json_response; + + return (status); +} + +Status Drone::Impl::GetEstimatedGeoLocation(GeoPosition* pgeo_position_out) { + Status status; + json json_response; + + if ((status = RequestDrone("GetEstimatedGeoLocation", &json_response)) == + Status::OK) + *pgeo_position_out = json_response; + + return (status); +} + +Status Drone::Impl::GetGroundTruthKinematics(json* pjson_out) const { + return (RequestDrone("GetGroundTruthKinematics", pjson_out)); +} + +Status Drone::Impl::GetEstimatedKinematics(json* pjson_out) const { + return (RequestDrone("GetEstimatedKinematics", pjson_out)); +} + +Status Drone::Impl::SetGroundTruthKinematics(const json& json_kinematics, + bool* pf_success_out) { + return (RequestDrone("SetGroundTruthKinematics", + json{{"kinematics", json_kinematics}}, pf_success_out)); +} + +Status Drone::Impl::SetControlSignals(const json& json_control_signal_map, + bool* pf_success_out) { + return (RequestDrone("SetControlSignals", + json{{"control_signal_map", json_control_signal_map}}, + pf_success_out)); +} + +Status Drone::Impl::UpdateActuatorFaultState(const std::string& actuator_id, + bool fault_configured, + bool* pf_success_out) { + return (RequestMethod(str_parent_topic_ + "/actuators/" + actuator_id + + "/ToggleFault", + json{{"enable", fault_configured}}, pf_success_out)); +} + +Status Drone::Impl::SetExternalForce(const std::vector& ext_force, + bool* pf_success_out) { + return (RequestDrone("SetExternalForce", json{{"ext_force", ext_force}}, + pf_success_out)); +} + +size_t Drone::Impl::GetRobotInfoCount(void) const { + return (vecstr_robot_info_name_.size()); +} + +const char* Drone::Impl::GetRobotInfoName(int iname) const { + const char* sz_ret = nullptr; + + if ((iname >= 0) && (iname < vecstr_robot_info_name_.size())) + sz_ret = vecstr_robot_info_name_[iname].c_str(); + + return (sz_ret); +} + +const char* Drone::Impl::GetRobotTopic(const std::string& str_info_name) const { + const char* sz_ret = nullptr; + auto it_robot = umrobot_name_topic_.find(str_info_name); + + if (it_robot != umrobot_name_topic_.end()) sz_ret = it_robot->second.c_str(); + + return (sz_ret); +} + +size_t Drone::Impl::GetSensorCount(void) const { + return (vecstr_sensor_name_.size()); +} + +size_t Drone::Impl::GetSensorInfoCount( + const std::string& str_sensor_name) const { + size_t cinfo = 0; + auto it = umsensor_name_vecinfo_.find(str_sensor_name); + + if (it != umsensor_name_vecinfo_.end()) cinfo = it->second.size(); + + return (cinfo); +} + +const char* Drone::Impl::GetSensorInfoName(const std::string& str_sensor_name, + int iinfo) const { + const char* sz_ret = nullptr; + auto it = umsensor_name_vecinfo_.find(str_sensor_name); + + if ((it != umsensor_name_vecinfo_.end()) && (iinfo >= 0) && + (iinfo < it->second.size())) { + sz_ret = it->second[iinfo].c_str(); + } + + return (sz_ret); +} + +const char* Drone::Impl::GetSensorName(int iname) const { + const char* sz_ret = nullptr; + + if ((iname >= 0) && (iname < vecstr_sensor_name_.size())) + sz_ret = vecstr_sensor_name_[iname].c_str(); + + return (sz_ret); +} + +const char* Drone::Impl::GetSensorTopic( + const std::string& str_sensor_name, + const std::string& str_info_name) const { + const char* sz_ret = nullptr; + auto it_sensor = umsensor_name_uminfo_topic_.find(str_sensor_name); + + if (it_sensor != umsensor_name_uminfo_topic_.end()) { + auto& uminfo_topic = it_sensor->second; + auto it_info = uminfo_topic.find(str_info_name); + + if (it_info != uminfo_topic.end()) sz_ret = it_info->second.c_str(); + } + + return (sz_ret); +} + +AsyncResult Drone::Impl::GoHomeAsync(float sec_timeout, float velocity, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"timeout_sec", sec_timeout}, + {"velocity", velocity}, + }; + + return (RequestDroneAsync("GoHome", json_params, fnresponse_callback)); +} + +AsyncResult Drone::Impl::HoverAsync(FnResponseCallback fnresponse_callback) { + return (RequestDroneAsync("Hover", json(), fnresponse_callback)); +} + +AsyncResult Drone::Impl::RequestControlAsync( + FnResponseCallback fnresponse_callback) { + return (RequestDroneAsync("RequestControl", json(), fnresponse_callback)); +} + +AsyncResult Drone::Impl::SetMissionModeAsync( + FnResponseCallback fnresponse_callback) { + return (RequestDroneAsync("SetMissionMode", json(), fnresponse_callback)); +} + +AsyncResult Drone::Impl::SetVTOLModeAsync( + VTOLMode vtol_mode, FnResponseCallback fnresponse_callback) { + return (RequestDroneAsync( + "SetVTOLMode", json{{"vtol_mode", static_cast(vtol_mode)}}, + fnresponse_callback)); +} + +Status Drone::Impl::Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& str_drone_name) { + Status status = Status::OK; + + log.InfoF("Initalizing Drone '%s'...", str_drone_name.c_str()); + + pclient_ = pclient; + pworld_ = pworld; + str_drone_name_ = str_drone_name; + str_world_parent_topic_ = pworld->GetParentTopic(); + + try { + SetTopics(); + LogTopics(); + + log.InfoF("Drone '%s' initialized for World scene '%s'", + str_drone_name_.c_str(), str_world_parent_topic_.c_str()); + } catch (Status status_caught) { + status = status_caught; + } + + if (status != Status::OK) { + char sz_error[512]; + + GetStatusString(status, sz_error); + log.ErrorF("Failed to initialize drone '%s': %s", str_drone_name.c_str(), + sz_error); + } + + return (status); +} + +Status Drone::Impl::IsAPIControlEnabled(bool* pf_is_enabled_out) const { + return (RequestDrone("IsApiControlEnabled", pf_is_enabled_out)); +} + +AsyncResult Drone::Impl::LandAsync(float sec_timeout, + FnResponseCallback fnresponse_callback) { + json json_params = {{"timeout_sec", sec_timeout}}; + + return (RequestDroneAsync("Land", json_params, fnresponse_callback)); +} + +AsyncResult Drone::Impl::MoveByVelocityAsync( + float v_north, float v_east, float v_down, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"vx", v_north}, + {"vy", v_east}, + {"vz", v_down}, + {"duration", sec_duration}, + {"drivetrain", yaw_control_mode}, + {"yaw_is_rate", yaw_is_rate}, + {"yaw", yaw}, + }; + + return ( + RequestDroneAsync("MoveByVelocity", json_params, fnresponse_callback)); +} + + AsyncResult Drone::Impl::MoveByVelocityZAsync( + float v_north, float v_east, float z, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"vx", v_north}, + {"vy", v_east}, + {"z", z}, + {"duration", sec_duration}, + {"drivetrain", yaw_control_mode}, + {"yaw_is_rate", yaw_is_rate}, + {"yaw", yaw}, + }; + + return ( + RequestDroneAsync("MoveByVelocityZ", json_params, fnresponse_callback)); + } + +AsyncResult Drone::Impl::MoveByVelocityBodyFrameAsync( + float v_forward, float v_right, float v_down, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"vx", v_forward}, + {"vy", v_right}, + {"vz", v_down}, + {"duration", sec_duration}, + {"drivetrain", yaw_control_mode}, + {"yaw_is_rate", yaw_is_rate}, + {"yaw", yaw}, + }; + + return (RequestDroneAsync("MoveByVelocityBodyFrame", json_params, + fnresponse_callback)); +} + +AsyncResult Drone::Impl::MoveByVelocityBodyFrameZAsync( + float v_forward, float v_right, float z, float sec_duration, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"vx", v_forward}, + {"vy", v_right}, + {"z", z}, + {"duration", sec_duration}, + {"drivetrain", yaw_control_mode}, + {"yaw_is_rate", yaw_is_rate}, + {"yaw", yaw}, + }; + + return (RequestDroneAsync("MoveByVelocityBodyFrameZ", json_params, + fnresponse_callback)); +} + +AsyncResult Drone::Impl::MoveByHeadingAsync( + float heading, float speed, float v_down, float sec_duration, + float heading_margin, float yaw_rate, float timeout_sec, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"heading", heading}, + {"speed", speed}, + {"vz", v_down}, + {"duration", sec_duration}, + {"heading_margin", heading_margin}, + {"yaw_rate", yaw_rate}, + {"timeout_sec", timeout_sec}, + }; + + return (RequestDroneAsync("MoveByHeading", json_params, + fnresponse_callback)); +} + +AsyncResult Drone::Impl::MoveOnPathAsync( + const VecVector3& path, float velocity, float timeout_sec, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"path", path}, + {"velocity", velocity}, + {"timeout_sec", timeout_sec}, + {"drivetrain", yaw_control_mode}, + {"yaw_is_rate", yaw_is_rate}, + {"yaw", yaw}, + {"lookahead", lookahead}, + {"adaptive_lookahead", adaptive_lookahead}, + }; + + return (RequestDroneAsync("MoveOnPath", json_params, fnresponse_callback)); +} + +AsyncResult Drone::Impl::MoveToPositionAsync( + float north, float east, float down, float velocity, float timeout_sec, + YawControlMode yaw_control_mode, bool yaw_is_rate, float yaw, + float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"x", north}, + {"y", east}, + {"z", down}, + {"velocity", velocity}, + {"timeout_sec", timeout_sec}, + {"drivetrain", yaw_control_mode}, + {"yaw_is_rate", yaw_is_rate}, + {"yaw", yaw}, + {"lookahead", lookahead}, + {"adaptive_lookahead", adaptive_lookahead}, + }; + + return ( + RequestDroneAsync("MoveToPosition", json_params, fnresponse_callback)); +} + +AsyncResult Drone::Impl::TakeoffAsync(float sec_timeout, + FnResponseCallback fnresponse_callback) { + json json_params = {{"timeout_sec", sec_timeout}}; + + return (RequestDroneAsync("Takeoff", json_params, fnresponse_callback)); +} + +AsyncResult Drone::Impl::RotateToYawAsync(float yaw, float timeout_sec, + float margin, float yaw_rate, + FnResponseCallback + fnresponse_callback) { + json json_params = { + {"yaw", yaw}, + {"timeout_sec", timeout_sec}, + {"margin", margin}, + {"yaw_rate", yaw_rate}, + }; + + return ( + RequestDroneAsync("RotateToYaw", json_params, fnresponse_callback)); +} + +AsyncResult Drone::Impl::RotateByYawRateAsync( + float yaw_rate, float sec_duration, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"yaw_rate", yaw_rate}, + {"duration", sec_duration}, + }; + + return (RequestDroneAsync("RotateByYawRate", json_params, + fnresponse_callback)); +} + +void Drone::Impl::LogTopics(void) const { + log.Info("-------------------------------------------------"); + log.InfoF("The following topics can be subscribed to for robot '%s':", + str_drone_name_.c_str()); + for (auto& pair_sensor : umsensor_name_uminfo_topic_) { + for (auto& pair_info : pair_sensor.second) + log.InfoF(" sensors[\"%s\"][\"%s\"]", pair_sensor.first.c_str(), + pair_info.first.c_str()); + } + for (auto& pair_robot : umrobot_name_topic_) + log.InfoF(" robot_info[\"%s\"]", pair_robot.first.c_str()); + log.Info("-------------------------------------------------"); +} + +Status Drone::Impl::RequestDrone(const std::string str_method, + const json& json_params) const { + Status status; + Message message_response; + + status = pclient_->Request(str_parent_topic_ + "/" + str_method, json_params, + &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() != 0) { + status = Status::RejectedByServer; + + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", str_method.c_str(), + response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status Drone::Impl::RequestDrone(const std::string str_method, + bool* pf_out) const { + return (RequestDrone(str_method, json(), pf_out)); +} + +Status Drone::Impl::RequestDrone(const std::string str_method, + json* pjson_out) const { + return (RequestDrone(str_method, json(), pjson_out)); +} + +Status Drone::Impl::RequestDrone(const std::string str_method, + const json& json_params, bool* pf_out) const { + json json_response; + Status status; + + status = RequestDrone(str_method, json_params, &json_response); + if (status == Status::OK) *pf_out = json_response; + + return (status); +} + +Status Drone::Impl::RequestDrone(const std::string str_method, + const json& json_params, + json* pjson_out) const { + Message message_response; + Status status; + + status = pclient_->Request(str_parent_topic_ + "/" + str_method, json_params, + &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() == 0) + *pjson_out = response_message.GetResult(); + else { + status = Status::RejectedByServer; + + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", str_method.c_str(), + response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +AsyncResult Drone::Impl::RequestDroneAsync( + const std::string str_method, const json& json_params, + FnResponseCallback fnresponse_callback) const { + return (internal::AsyncResultMessageConverter(pclient_->RequestAsync( + str_parent_topic_ + "/" + str_method, json_params, fnresponse_callback))); +} + +Status Drone::Impl::RequestMethod(const std::string& str_method_path, + const json& json_params, + json* pjson_out) const { + Message message_response; + Status status; + + status = pclient_->Request(str_method_path, json_params, &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() == 0) + *pjson_out = response_message.GetResult(); + else { + status = Status::RejectedByServer; + + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", + str_method_path.c_str(), response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status Drone::Impl::RequestMethod(const std::string& str_method_path, + const json& json_params, + bool* pf_out) const { + json json_response; + Status status; + + status = RequestMethod(str_method_path, json_params, &json_response); + if (status == Status::OK) *pf_out = json_response; + + return (status); +} + +Status Drone::Impl::RequestMethod(const std::string& str_method_path, + const json& json_params, + float* pf_out) const { + json json_response; + Status status; + + status = RequestMethod(str_method_path, json_params, &json_response); + if (status == Status::OK) *pf_out = json_response.get(); + + return (status); +} + +Status Drone::Impl::RequestMethod(const std::string& str_method_path, + const json& json_params) const { + json json_response; + + return (RequestMethod(str_method_path, json_params, &json_response)); +} + +Status Drone::Impl::RequestSensorData(const std::string& str_sensor_name, + const std::string& str_sensor_info_name, + json* pjson_out) const { + auto sz_method_path = + GetSensorTopic(str_sensor_name, str_sensor_info_name); + + if (sz_method_path == nullptr) return (Status::NotFound); + + return (RequestMethod(sz_method_path, json(), pjson_out)); +} + +Status Drone::Impl::SetPose(const Transform& transform, bool reset_kinematics) { + json json_params = {{"pose", transform}, + {"reset_kinematics", reset_kinematics}}; + + return (RequestDrone("SetPose", json_params)); +} + +void Drone::Impl::SetRobotInfoTopics(void) { + // TODO: Change this to construct this info from the published topics rather + // than + // hard coding it. + + const char* c_sz_robot_info_name[3] = { + "actual_pose", + "collision_info", + "rotor_info", + }; + + umrobot_name_topic_.clear(); + vecstr_robot_info_name_.clear(); + for (auto& sz : c_sz_robot_info_name) { + umrobot_name_topic_.insert( + std::make_pair(sz, str_parent_topic_ + "/" + sz)); + vecstr_robot_info_name_.push_back(sz); + } +} + +#ifdef ORIG_IMPL // Implementation as done in Drone.py +void Drone::Impl::SetSensorTopics(World* pworld) { + // TODO: Change this to construct this info from the published topics rather + // than + // extracting it from the configuration. + + static const std::unordered_map c_umcapture_name_setting = { + {0, "scene_camera"}, // TODO: rename scene_camera topic to rgb_camera + {1, "depth_camera"}, + {2, "depth_planar_camera"}, + {3, "segmentation_camera"}, + {4, "depth_vis_camera"}, + {5, "disparity_normalized_camera"}, + {6, "surface_normals_camera"}, + }; + + auto& json_config = pworld->GetConfiguration(); + json json_actor_config; + + auto fnAddSensorInfo = [this](std::string sensor_name, std::string info_name, + std::string topic_root_name) { + auto it = umsensor_name_uminfo_topic_.find(sensor_name); + + if (it == umsensor_name_uminfo_topic_.end()) { + auto pair = umsensor_name_uminfo_topic_.emplace(sensor_name, UMStrStr()); + + it = pair.first; + vecstr_sensor_name_.push_back(sensor_name); + umsensor_name_vecinfo_.emplace(sensor_name, VecStr()); + } + + it->second.insert( + std::make_pair(info_name, topic_root_name + "/" + info_name)); + umsensor_name_vecinfo_[sensor_name].push_back(info_name); + }; + + umsensor_name_uminfo_topic_.clear(); + umsensor_name_vecinfo_.clear(); + vecstr_sensor_name_.clear(); + + // Get the config for this drone + if (!json_config.is_null() && json_config["actors"].is_array()) { + for (auto& json_actor : json_config["actors"]) { + if (json_actor["name"] == str_drone_name_) { + json_actor_config = json_actor["robot-config"]; + break; + } + } + } + + if (json_actor_config.is_null()) { + log.ErrorF("Actor \"%s\" not found in the config", str_drone_name_.c_str()); + throw Status::NotFound; + } + + // Loop through the robot's sensors + if (!json_actor_config["sensors"].is_array()) return; + + for (auto& json_sensor : json_actor_config["sensors"]) { + // Add topic entries for each recognized sensor type + std::string str_name = json_sensor["id"]; + std::string str_sensor_type = json_sensor["type"]; + std::string str_sensor_topic_root = str_sensor_topic_ + "/" + str_name; + + if (str_sensor_type == "airspeed") + fnAddSensorInfo(str_name, "airspeed", str_sensor_topic_root); + else if (str_sensor_type == "barometer") + fnAddSensorInfo(str_name, "barometer", str_sensor_topic_root); + else if (str_sensor_type == "battery") + fnAddSensorInfo(str_name, "battery", str_sensor_topic_root); + else if (str_sensor_type == "camera") { + auto json_subcameras = json_sensor["capture-settings"]; + + if (json_subcameras.is_array()) { + for (auto& json_subcamera : json_subcameras) { + auto i_image_type = json_subcamera["image-type"]; + auto it = c_umcapture_name_setting.find(i_image_type); + + if (it != c_umcapture_name_setting.end()) { + auto str_image_type = it->second; + + fnAddSensorInfo(str_name, str_image_type, str_sensor_topic_root); + fnAddSensorInfo(str_name, str_image_type + "_info", + str_sensor_topic_root); + } + } + } + } else if (str_sensor_type == "distance-sensor") + fnAddSensorInfo(str_name, "distance-sensor", str_sensor_topic_root); + else if (str_sensor_type == "gps") + fnAddSensorInfo(str_name, "gps", str_sensor_topic_root); + else if (str_sensor_type == "imu") + fnAddSensorInfo(str_name, "imu_kinematics", str_sensor_topic_root); + else if (str_sensor_type == "lidar") + fnAddSensorInfo(str_name, "lidar", str_sensor_topic_root); + else if (str_sensor_type == "magnetometer") + fnAddSensorInfo(str_name, "magnetometer", str_sensor_topic_root); + else if (str_sensor_type == "radar") { + fnAddSensorInfo(str_name, "radar_detections", str_sensor_topic_root); + fnAddSensorInfo(str_name, "radar_tracks", str_sensor_topic_root); + } else { + log.ErrorF("Unknown sensor type \"%s\" found in config for sensor \"%s\"", + str_sensor_type.c_str(), str_name.c_str()); + throw Status::NotFound; + } + } +} +#endif // ORIG_IMPL + +void Drone::Impl::SetSensorTopics(World* /*pworld*/) { + bool fdrone_found = false; + std::regex re_drone("^/Sim/.+/robots/([^/]+)/.+$"); + std::regex re_sensor("^/Sim/.+/sensors/([^/]+)/([^/]+)$"); + auto vec_str_topic = pclient_->GetTopicInfo(); + std::unordered_set us_sensor; + + // Clear existing sensor lists + umsensor_name_uminfo_topic_.clear(); + umsensor_name_vecinfo_.clear(); + vecstr_sensor_name_.clear(); + + // Loop through the robot's sensor topics + for (auto& str_topic : vec_str_topic) { + std::smatch smatch_drone; + + // Ignore topics except those for this drone + if (std::regex_match(str_topic, smatch_drone, re_drone)) { + std::ssub_match ssub_match_drone = smatch_drone[1]; + + if (ssub_match_drone.str() == str_drone_name_) { + std::smatch smatch_sensor; + + fdrone_found = true; + + // Ignore topics except those for the sensors + if (std::regex_match(str_topic, smatch_sensor, re_sensor)) { + // Add topic entries for each recognized sensor type + std::string str_sensor_name = smatch_sensor[1].str(); + std::string str_sensor_info_name = smatch_sensor[2].str(); + + // Add sensor name to list + us_sensor.insert(str_sensor_name); + + // Add sensor info name to sensor's list + { + auto it_info = umsensor_name_vecinfo_.find(str_sensor_name); + + if (it_info == umsensor_name_vecinfo_.end()) { + auto pair = + umsensor_name_vecinfo_.emplace(str_sensor_name, VecStr()); + + it_info = pair.first; + } + it_info->second.push_back(str_sensor_info_name); + } + + // Add sensor info topic to sensor's list + { + auto it_um_info = umsensor_name_uminfo_topic_.find(str_sensor_name); + + if (it_um_info == umsensor_name_uminfo_topic_.end()) { + auto pair = umsensor_name_uminfo_topic_.insert( + std::make_pair(str_sensor_name, UMStrStr())); + + it_um_info = pair.first; + } + it_um_info->second.insert( + std::make_pair(str_sensor_info_name, str_topic)); + } + } + } + } + } + + if (!fdrone_found) { + log.ErrorF("Actor \"%s\" not found in the config", str_drone_name_.c_str()); + throw Status::NotFound; + } +} + +void Drone::Impl::SetTopics(void) { + str_parent_topic_ = str_world_parent_topic_ + "/robots/" + str_drone_name_; + str_sensor_topic_ = str_parent_topic_ + "/sensors"; + SetSensorTopics(pworld_.get()); + SetRobotInfoTopics(); +} + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/ProjectAirsimClientLib/src/EnvActor.cpp b/client/cpp/ProjectAirsimClientLib/src/EnvActor.cpp new file mode 100644 index 00000000..98caf7ed --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/src/EnvActor.cpp @@ -0,0 +1,259 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include "EnvActor.h" + +#include +#include + +#include "ProjectAirsimClient.h" +#include "ProjectAirSimMessage/response_message.hpp" +#include "pch.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class EnvActor::Impl { + public: + Impl(void) noexcept; + ~Impl(); + + Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& actor_name); + Status SetTrajectory(const std::string& traj_name, bool to_loop, + float time_offset, float x_offset, float y_offset, + float z_offset, float roll_offset, float pitch_offset, + float yaw_offset); + Status SetLinkRotationAngle(const std::string& link_name, float angle_deg); + Status SetLinkRotationRate(const std::string& link_name, + float rotation_deg_per_sec); + Status SetLinkRotationAngles( + const std::map& rotation_angle_map); + Status SetLinkRotationRates( + const std::map& rotation_rate_map); + + protected: + Status RequestActor(const std::string& str_method_path, + const json& json_params = json()) const; + Status RequestActor(const std::string& str_method_path, + const json& json_params, json* pjson_out) const; + Status RequestActor(const std::string& str_method_path, + const json& json_params, bool* pf_out) const; + + protected: + std::shared_ptr pclient_; + std::string str_actor_name_; + std::string str_parent_topic_; // {world}/env_actors/{name} + std::string str_world_parent_topic_; // {world} +}; // class EnvActor::Impl + +// --------------------------------------------------------------------------- +// EnvActor public API +// --------------------------------------------------------------------------- + +ASC_DECL EnvActor::EnvActor(void) noexcept : pimpl_(new Impl()) {} + +ASC_DECL EnvActor::~EnvActor() {} + +ASC_DECL Status EnvActor::Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& actor_name) { + RETURN_CATCH_STATUS(pimpl_->Initialize(pclient, pworld, actor_name)); +} + +ASC_DECL Status EnvActor::SetTrajectory(const std::string& traj_name, + bool to_loop, float time_offset, + float x_offset, float y_offset, + float z_offset, float roll_offset, + float pitch_offset, + float yaw_offset) { + RETURN_CATCH_STATUS(pimpl_->SetTrajectory(traj_name, to_loop, time_offset, + x_offset, y_offset, z_offset, + roll_offset, pitch_offset, + yaw_offset)); +} + +ASC_DECL Status EnvActor::SetLinkRotationAngle(const std::string& link_name, + float angle_deg) { + RETURN_CATCH_STATUS(pimpl_->SetLinkRotationAngle(link_name, angle_deg)); +} + +ASC_DECL Status EnvActor::SetLinkRotationRate( + const std::string& link_name, float rotation_deg_per_sec) { + RETURN_CATCH_STATUS( + pimpl_->SetLinkRotationRate(link_name, rotation_deg_per_sec)); +} + +ASC_DECL Status EnvActor::SetLinkRotationAngles( + const std::map& rotation_angle_map) { + RETURN_CATCH_STATUS(pimpl_->SetLinkRotationAngles(rotation_angle_map)); +} + +ASC_DECL Status EnvActor::SetLinkRotationRates( + const std::map& rotation_rate_map) { + RETURN_CATCH_STATUS(pimpl_->SetLinkRotationRates(rotation_rate_map)); +} + +// --------------------------------------------------------------------------- +// EnvActor::Impl +// --------------------------------------------------------------------------- + +EnvActor::Impl::Impl() noexcept + : pclient_(), + str_actor_name_(), + str_parent_topic_(), + str_world_parent_topic_() {} + +EnvActor::Impl::~Impl() {} + +Status EnvActor::Impl::Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& actor_name) { + log.InfoF("Initalizing EnvActor '%s'...", actor_name.c_str()); + + pclient_ = pclient; + str_actor_name_ = actor_name; + str_world_parent_topic_ = pworld->GetParentTopic(); + str_parent_topic_ = + str_world_parent_topic_ + "/env_actors/" + str_actor_name_; + + log.InfoF("EnvActor '%s' initialized for World scene '%s'", + str_actor_name_.c_str(), str_world_parent_topic_.c_str()); + + return (Status::OK); +} + +Status EnvActor::Impl::SetTrajectory(const std::string& traj_name, + bool to_loop, float time_offset, + float x_offset, float y_offset, + float z_offset, float roll_offset, + float pitch_offset, float yaw_offset) { + // SetEnvActorTrajectory is dispatched on the world topic, not the actor topic + json json_params = { + {"env_actor_name", str_actor_name_}, + {"traj_name", traj_name}, + {"time_offset", time_offset}, + {"x_offset", x_offset}, + {"y_offset", y_offset}, + {"z_offset", z_offset}, + {"roll_offset", roll_offset}, + {"pitch_offset", pitch_offset}, + {"yaw_offset", yaw_offset}, + {"to_loop", to_loop}, + }; + + return (RequestActor(str_world_parent_topic_ + "/SetEnvActorTrajectory", + json_params)); +} + +Status EnvActor::Impl::SetLinkRotationAngle(const std::string& link_name, + float angle_deg) { + json json_params = { + {"env_actor_name", str_actor_name_}, + {"link_name", link_name}, + {"angle_deg", angle_deg}, + }; + return (RequestActor(str_world_parent_topic_ + "/SetEnvActorLinkRotAngle", + json_params)); +} + +Status EnvActor::Impl::SetLinkRotationRate(const std::string& link_name, + float rotation_deg_per_sec) { + json json_params = { + {"env_actor_name", str_actor_name_}, + {"link_name", link_name}, + {"rotation_deg_per_sec", rotation_deg_per_sec}, + }; + return (RequestActor(str_world_parent_topic_ + "/SetEnvActorLinkRotRate", + json_params)); +} + +Status EnvActor::Impl::SetLinkRotationAngles( + const std::map& rotation_angle_map) { + for (const auto& kv : rotation_angle_map) { + Status status = SetLinkRotationAngle(kv.first, kv.second); + if (status != Status::OK) return status; + } + return Status::OK; +} + +Status EnvActor::Impl::SetLinkRotationRates( + const std::map& rotation_rate_map) { + for (const auto& kv : rotation_rate_map) { + Status status = SetLinkRotationRate(kv.first, kv.second); + if (status != Status::OK) return status; + } + return Status::OK; +} + +// --------------------------------------------------------------------------- +// Request helpers +// --------------------------------------------------------------------------- + +Status EnvActor::Impl::RequestActor(const std::string& str_method_path, + const json& json_params) const { + Status status; + Message message_response; + + status = pclient_->Request(str_method_path, json_params, &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() != 0) { + status = Status::RejectedByServer; + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", + str_method_path.c_str(), response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status EnvActor::Impl::RequestActor(const std::string& str_method_path, + const json& json_params, + json* pjson_out) const { + Status status; + Message message_response; + + status = pclient_->Request(str_method_path, json_params, &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() == 0) + *pjson_out = response_message.GetResult(); + else { + status = Status::RejectedByServer; + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", + str_method_path.c_str(), response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status EnvActor::Impl::RequestActor(const std::string& str_method_path, + const json& json_params, + bool* pf_out) const { + json json_response; + Status status; + + status = RequestActor(str_method_path, json_params, &json_response); + if (status == Status::OK) *pf_out = json_response; + + return (status); +} + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/AirSimClientDLL/src/ICommChannel.cpp b/client/cpp/ProjectAirsimClientLib/src/ICommChannel.cpp similarity index 100% rename from client/cpp/AirSimClientDLL/src/ICommChannel.cpp rename to client/cpp/ProjectAirsimClientLib/src/ICommChannel.cpp diff --git a/client/cpp/AirSimClientDLL/src/ICommChannel.h b/client/cpp/ProjectAirsimClientLib/src/ICommChannel.h similarity index 100% rename from client/cpp/AirSimClientDLL/src/ICommChannel.h rename to client/cpp/ProjectAirsimClientLib/src/ICommChannel.h diff --git a/client/cpp/AirSimClientDLL/src/IMessageBuffer.h b/client/cpp/ProjectAirsimClientLib/src/IMessageBuffer.h similarity index 100% rename from client/cpp/AirSimClientDLL/src/IMessageBuffer.h rename to client/cpp/ProjectAirsimClientLib/src/IMessageBuffer.h diff --git a/client/cpp/AirSimClientDLL/src/JSONUtils.h b/client/cpp/ProjectAirsimClientLib/src/JSONUtils.h similarity index 80% rename from client/cpp/AirSimClientDLL/src/JSONUtils.h rename to client/cpp/ProjectAirsimClientLib/src/JSONUtils.h index 25065c74..2a44458b 100644 --- a/client/cpp/AirSimClientDLL/src/JSONUtils.h +++ b/client/cpp/ProjectAirsimClientLib/src/JSONUtils.h @@ -11,6 +11,17 @@ namespace microsoft { namespace projectairsim { namespace client { +inline void from_json(const nlohmann::json& j, ReadyState& ready_state) { + ready_state.is_ready = j.at("ready_val"); + ready_state.message = j.at("ready_message"); +} + +inline void from_json(const nlohmann::json& j, GeoPosition& geo_position) { + geo_position.latitude = j.at("latitude"); + geo_position.longitude = j.at("longitude"); + geo_position.altitude = j.at("altitude"); +} + inline void from_json(const nlohmann::json& j, Transform& transform) { nlohmann::json json_rotation = j.at("rotation"); nlohmann::json json_translation = j.at("translation"); @@ -39,6 +50,12 @@ inline void to_json(nlohmann::json& j, const ColorRGBA& colorrgba) { {colorrgba.red, colorrgba.green, colorrgba.blue, colorrgba.alpha}); } +inline void to_json(nlohmann::json& j, const GeoPosition& geo_position) { + j["latitude"] = geo_position.latitude; + j["longitude"] = geo_position.longitude; + j["altitude"] = geo_position.altitude; +} + inline void to_json(nlohmann::json& j, const Quaternion& quat) { j = nlohmann::json::array({quat.w, quat.x, quat.y, quat.z}); } diff --git a/client/cpp/AirSimClientDLL/src/Log.cpp b/client/cpp/ProjectAirsimClientLib/src/Log.cpp similarity index 98% rename from client/cpp/AirSimClientDLL/src/Log.cpp rename to client/cpp/ProjectAirsimClientLib/src/Log.cpp index eff60fab..72c3d080 100644 --- a/client/cpp/AirSimClientDLL/src/Log.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/Log.cpp @@ -10,7 +10,7 @@ #include #include -#include "AirSimClient.h" +#include "ProjectAirsimClient.h" #include "pch.h" #ifdef _WIN32 @@ -96,7 +96,7 @@ void Log::GetTimestamp(char(buf_out)[13]) noexcept { if (timespec_get(&ts, TIME_UTC) == 0) strcpy(buf_out, "unknown "); else { - auto milliseconds = ts.tv_nsec / 1000 / 1000; + int milliseconds = static_cast(ts.tv_nsec / 1000 / 1000); auto tm_cur = *localtime(&ts.tv_sec); sprintf(buf_out, "%02d:%02d:%02d:%03d", tm_cur.tm_hour, tm_cur.tm_min, diff --git a/client/cpp/AirSimClientDLL/src/Message.cpp b/client/cpp/ProjectAirsimClientLib/src/Message.cpp similarity index 100% rename from client/cpp/AirSimClientDLL/src/Message.cpp rename to client/cpp/ProjectAirsimClientLib/src/Message.cpp diff --git a/client/cpp/AirSimClientDLL/src/AirSimClient.cpp b/client/cpp/ProjectAirsimClientLib/src/ProjectAirsimClient.cpp similarity index 91% rename from client/cpp/AirSimClientDLL/src/AirSimClient.cpp rename to client/cpp/ProjectAirsimClientLib/src/ProjectAirsimClient.cpp index 8d48ac88..b510df31 100644 --- a/client/cpp/AirSimClientDLL/src/AirSimClient.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/ProjectAirsimClient.cpp @@ -3,7 +3,7 @@ // // MIT License. All rights reserved. -#include "AirSimClient.h" +#include "ProjectAirsimClient.h" #include diff --git a/client/cpp/AirSimClientDLL/src/RefCounted.cpp b/client/cpp/ProjectAirsimClientLib/src/RefCounted.cpp similarity index 100% rename from client/cpp/AirSimClientDLL/src/RefCounted.cpp rename to client/cpp/ProjectAirsimClientLib/src/RefCounted.cpp diff --git a/client/cpp/AirSimClientDLL/src/RefCounted.h b/client/cpp/ProjectAirsimClientLib/src/RefCounted.h similarity index 100% rename from client/cpp/AirSimClientDLL/src/RefCounted.h rename to client/cpp/ProjectAirsimClientLib/src/RefCounted.h diff --git a/client/cpp/ProjectAirsimClientLib/src/Rover.cpp b/client/cpp/ProjectAirsimClientLib/src/Rover.cpp new file mode 100644 index 00000000..525cdd77 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/src/Rover.cpp @@ -0,0 +1,365 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include "Rover.h" + +#include +#include + +#include "ProjectAirsimClient.h" +#include "ProjectAirSimMessage/response_message.hpp" +#include "AsyncResultInternal.h" +#include "JSONUtils.h" +#include "pch.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class Rover::Impl { + public: + Impl(void) noexcept; + ~Impl(); + + Status Arm(bool* pf_is_armed_out); + Status CanArm(bool* pf_can_be_armed_out) const; + Status CancelLastTask(bool* pf_task_is_canceled_out); + Status DisableAPIControl(bool* pf_is_disabled_out); + Status Disarm(bool* pf_is_disarmed_out); + Status EnableAPIControl(bool* pf_is_enabled_out); + Status IsAPIControlEnabled(bool* pf_is_enabled_out) const; + Status GetGroundTruthKinematics(json* pjson_out) const; + Status SetPose(const Transform& transform, bool reset_kinematics); + Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& rover_name); + AsyncResult MoveToPositionAsync(float north, float east, float velocity, + float timeout_sec, float yaw_rate_max, + float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback); + AsyncResult MoveByHeadingAsync(float heading, float speed, float sec_duration, + float heading_margin, float yaw_rate, + float timeout_sec, + FnResponseCallback fnresponse_callback); + Status SetRoverControls(float engine, float steering_angle, float brake); + + protected: + Status RequestRover(const std::string& str_method, + const json& json_params = json()) const; + Status RequestRover(const std::string& str_method, bool* pf_out) const; + Status RequestRover(const std::string& str_method, json* pjson_out) const; + Status RequestRover(const std::string& str_method, const json& json_params, + bool* pf_out) const; + Status RequestRover(const std::string& str_method, const json& json_params, + json* pjson_out) const; + AsyncResult RequestRoverAsync(const std::string& str_method, + const json& json_params, + FnResponseCallback fnresponse_callback) const; + + protected: + std::shared_ptr pclient_; + std::shared_ptr pworld_; + std::string str_rover_name_; + std::string str_parent_topic_; + std::string str_world_parent_topic_; +}; // class Rover::Impl + +// --------------------------------------------------------------------------- +// Rover public API +// --------------------------------------------------------------------------- + +ASC_DECL const float Rover::kNoTimeout = std::numeric_limits::max(); + +ASC_DECL Rover::Rover(void) noexcept : pimpl_(new Impl()) {} + +ASC_DECL Rover::~Rover() {} + +ASC_DECL Status Rover::Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& rover_name) { + RETURN_CATCH_STATUS(pimpl_->Initialize(pclient, pworld, rover_name)); +} + +ASC_DECL Status Rover::Arm(bool* pf_is_armed_out) { + RETURN_CATCH_STATUS(pimpl_->Arm(pf_is_armed_out)); +} + +ASC_DECL Status Rover::CanArm(bool* pf_can_be_armed_out) const { + RETURN_CATCH_STATUS(pimpl_->CanArm(pf_can_be_armed_out)); +} + +ASC_DECL Status Rover::CancelLastTask(bool* pf_task_is_canceled_out) { + RETURN_CATCH_STATUS(pimpl_->CancelLastTask(pf_task_is_canceled_out)); +} + +ASC_DECL Status Rover::DisableAPIControl(bool* pf_is_disabled_out) { + RETURN_CATCH_STATUS(pimpl_->DisableAPIControl(pf_is_disabled_out)); +} + +ASC_DECL Status Rover::Disarm(bool* pf_is_disarmed_out) { + RETURN_CATCH_STATUS(pimpl_->Disarm(pf_is_disarmed_out)); +} + +ASC_DECL Status Rover::EnableAPIControl(bool* pf_is_enabled_out) { + RETURN_CATCH_STATUS(pimpl_->EnableAPIControl(pf_is_enabled_out)); +} + +ASC_DECL Status Rover::IsAPIControlEnabled(bool* pf_is_enabled_out) const { + RETURN_CATCH_STATUS(pimpl_->IsAPIControlEnabled(pf_is_enabled_out)); +} + +ASC_DECL Status Rover::GetGroundTruthKinematics(json* pjson_out) const { + RETURN_CATCH_STATUS(pimpl_->GetGroundTruthKinematics(pjson_out)); +} + +ASC_DECL Status Rover::SetPose(const Transform& transform, + bool reset_kinematics) { + RETURN_CATCH_STATUS(pimpl_->SetPose(transform, reset_kinematics)); +} + +ASC_DECL AsyncResult Rover::MoveToPositionAsync( + float north, float east, float velocity, float timeout_sec, + float yaw_rate_max, float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveToPositionAsync(north, east, velocity, timeout_sec, + yaw_rate_max, lookahead, + adaptive_lookahead, fnresponse_callback)); +} + +ASC_DECL AsyncResult Rover::MoveByHeadingAsync( + float heading, float speed, float sec_duration, float heading_margin, + float yaw_rate, float timeout_sec, FnResponseCallback fnresponse_callback) { + return (pimpl_->MoveByHeadingAsync(heading, speed, sec_duration, + heading_margin, yaw_rate, timeout_sec, + fnresponse_callback)); +} + +ASC_DECL Status Rover::SetRoverControls(float engine, float steering_angle, + float brake) { + RETURN_CATCH_STATUS(pimpl_->SetRoverControls(engine, steering_angle, brake)); +} + +// --------------------------------------------------------------------------- +// Rover::Impl +// --------------------------------------------------------------------------- + +Rover::Impl::Impl() noexcept + : pclient_(), + pworld_(), + str_rover_name_(), + str_parent_topic_(), + str_world_parent_topic_() {} + +Rover::Impl::~Impl() {} + +Status Rover::Impl::Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& rover_name) { + log.InfoF("Initalizing Rover '%s'...", rover_name.c_str()); + + pclient_ = pclient; + pworld_ = pworld; + str_rover_name_ = rover_name; + str_world_parent_topic_ = pworld->GetParentTopic(); + str_parent_topic_ = str_world_parent_topic_ + "/robots/" + str_rover_name_; + + log.InfoF("Rover '%s' initialized for World scene '%s'", + str_rover_name_.c_str(), str_world_parent_topic_.c_str()); + + return (Status::OK); +} + +Status Rover::Impl::Arm(bool* pf_is_armed_out) { + return (RequestRover("Arm", pf_is_armed_out)); +} + +Status Rover::Impl::CanArm(bool* pf_can_be_armed_out) const { + return (RequestRover("CanArm", pf_can_be_armed_out)); +} + +Status Rover::Impl::CancelLastTask(bool* pf_task_is_canceled_out) { + TAsyncResult ar; + Status status; + + ar = pclient_->RequestPriorityAsync(str_parent_topic_ + "/CancelLastTask", + json(), nullptr); + if ((status = ar.Wait()) == Status::OK) { + const Message& message = ar.GetResult(); + ResponseMessage response_message; + + response_message.Deserialize(message); + if (response_message.GetErrorCode() == 0) + *pf_task_is_canceled_out = + response_message.GetResult().is_boolean() && + static_cast(response_message.GetResult()); + else { + status = Status::RejectedByServer; + log.ErrorF("Server method \"CancelLastTask\" failed: error %d: %s", + response_message.GetErrorCode(), + static_cast( + response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status Rover::Impl::DisableAPIControl(bool* pf_is_disabled_out) { + return (RequestRover("DisableApiControl", pf_is_disabled_out)); +} + +Status Rover::Impl::Disarm(bool* pf_is_disarmed_out) { + return (RequestRover("Disarm", pf_is_disarmed_out)); +} + +Status Rover::Impl::EnableAPIControl(bool* pf_is_enabled_out) { + return (RequestRover("EnableApiControl", pf_is_enabled_out)); +} + +Status Rover::Impl::IsAPIControlEnabled(bool* pf_is_enabled_out) const { + return (RequestRover("IsApiControlEnabled", pf_is_enabled_out)); +} + +Status Rover::Impl::GetGroundTruthKinematics(json* pjson_out) const { + return (RequestRover("GetGroundTruthKinematics", pjson_out)); +} + +Status Rover::Impl::SetPose(const Transform& transform, bool reset_kinematics) { + json json_params = {{"pose", transform}, + {"reset_kinematics", reset_kinematics}}; + return (RequestRover("SetPose", json_params)); +} + +AsyncResult Rover::Impl::MoveToPositionAsync( + float north, float east, float velocity, float timeout_sec, + float yaw_rate_max, float lookahead, float adaptive_lookahead, + FnResponseCallback fnresponse_callback) { + json json_params = { + {"x", north}, + {"y", east}, + {"velocity", velocity}, + {"timeout_sec", timeout_sec}, + {"yaw_rate_max", yaw_rate_max}, + {"lookahead", lookahead}, + {"adaptive_lookahead", adaptive_lookahead}, + }; + + return (RequestRoverAsync("MoveToPosition", json_params, fnresponse_callback)); +} + +AsyncResult Rover::Impl::MoveByHeadingAsync(float heading, float speed, + float sec_duration, + float heading_margin, + float yaw_rate, float timeout_sec, + FnResponseCallback + fnresponse_callback) { + json json_params = { + {"heading", heading}, + {"speed", speed}, + {"duration", sec_duration}, + {"heading_margin", heading_margin}, + {"yaw_rate", yaw_rate}, + {"timeout_sec", timeout_sec}, + }; + + return (RequestRoverAsync("MoveByHeading", json_params, fnresponse_callback)); +} + +Status Rover::Impl::SetRoverControls(float engine, float steering_angle, + float brake) { + json json_params = { + {"engine", engine}, + {"steering_angle", steering_angle}, + {"brake", brake}, + }; + return (RequestRover("SetRoverControls", json_params)); +} + +// --------------------------------------------------------------------------- +// Request helpers +// --------------------------------------------------------------------------- + +Status Rover::Impl::RequestRover(const std::string& str_method, + const json& json_params) const { + Status status; + Message message_response; + + status = pclient_->Request(str_parent_topic_ + "/" + str_method, json_params, + &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() != 0) { + status = Status::RejectedByServer; + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", str_method.c_str(), + response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +Status Rover::Impl::RequestRover(const std::string& str_method, + bool* pf_out) const { + return (RequestRover(str_method, json(), pf_out)); +} + +Status Rover::Impl::RequestRover(const std::string& str_method, + json* pjson_out) const { + return (RequestRover(str_method, json(), pjson_out)); +} + +Status Rover::Impl::RequestRover(const std::string& str_method, + const json& json_params, bool* pf_out) const { + json json_response; + Status status; + + status = RequestRover(str_method, json_params, &json_response); + if (status == Status::OK) *pf_out = json_response; + + return (status); +} + +Status Rover::Impl::RequestRover(const std::string& str_method, + const json& json_params, + json* pjson_out) const { + Message message_response; + Status status; + + status = pclient_->Request(str_parent_topic_ + "/" + str_method, json_params, + &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() == 0) + *pjson_out = response_message.GetResult(); + else { + status = Status::RejectedByServer; + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", str_method.c_str(), + response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +AsyncResult Rover::Impl::RequestRoverAsync( + const std::string& str_method, const json& json_params, + FnResponseCallback fnresponse_callback) const { + return (internal::AsyncResultMessageConverter(pclient_->RequestAsync( + str_parent_topic_ + "/" + str_method, json_params, fnresponse_callback))); +} + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/ProjectAirsimClientLib/src/StaticSensorActor.cpp b/client/cpp/ProjectAirsimClientLib/src/StaticSensorActor.cpp new file mode 100644 index 00000000..ad09e0b4 --- /dev/null +++ b/client/cpp/ProjectAirsimClientLib/src/StaticSensorActor.cpp @@ -0,0 +1,130 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include "StaticSensorActor.h" + +#include +#include + +#include "ProjectAirsimClient.h" +#include "ProjectAirSimMessage/response_message.hpp" +#include "pch.h" + +namespace microsoft { +namespace projectairsim { +namespace client { + +class StaticSensorActor::Impl { + public: + Impl(void) noexcept; + ~Impl(); + + Status Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& actor_name); + Status GetImages(const std::string& str_camera_id, + const std::vector& veci_image_type_id, + json* pjson_out) const; + + protected: + Status RequestMethod(const std::string& str_method_path, + const json& json_params, json* pjson_out) const; + + protected: + std::shared_ptr pclient_; + std::string str_actor_name_; + std::string str_parent_topic_; // {world}/robots/{name} + std::string str_sensor_topic_; // {world}/robots/{name}/sensors +}; // class StaticSensorActor::Impl + +// --------------------------------------------------------------------------- +// StaticSensorActor public API +// --------------------------------------------------------------------------- + +ASC_DECL StaticSensorActor::StaticSensorActor(void) noexcept + : pimpl_(new Impl()) {} + +ASC_DECL StaticSensorActor::~StaticSensorActor() {} + +ASC_DECL Status StaticSensorActor::Initialize( + std::shared_ptr& pclient, std::shared_ptr& pworld, + const std::string& actor_name) { + RETURN_CATCH_STATUS(pimpl_->Initialize(pclient, pworld, actor_name)); +} + +ASC_DECL Status StaticSensorActor::GetImages( + const std::string& str_camera_id, + const std::vector& veci_image_type_id, json* pjson_out) const { + RETURN_CATCH_STATUS( + pimpl_->GetImages(str_camera_id, veci_image_type_id, pjson_out)); +} + +// --------------------------------------------------------------------------- +// StaticSensorActor::Impl +// --------------------------------------------------------------------------- + +StaticSensorActor::Impl::Impl() noexcept + : pclient_(), + str_actor_name_(), + str_parent_topic_(), + str_sensor_topic_() {} + +StaticSensorActor::Impl::~Impl() {} + +Status StaticSensorActor::Impl::Initialize(std::shared_ptr& pclient, + std::shared_ptr& pworld, + const std::string& actor_name) { + log.InfoF("Initalizing StaticSensorActor '%s'...", actor_name.c_str()); + + pclient_ = pclient; + str_actor_name_ = actor_name; + const std::string world_topic = pworld->GetParentTopic(); + str_parent_topic_ = world_topic + "/robots/" + str_actor_name_; + str_sensor_topic_ = str_parent_topic_ + "/sensors"; + + log.InfoF("StaticSensorActor '%s' initialized for World scene '%s'", + str_actor_name_.c_str(), world_topic.c_str()); + + return (Status::OK); +} + +Status StaticSensorActor::Impl::GetImages( + const std::string& str_camera_id, + const std::vector& veci_image_type_id, json* pjson_out) const { + json json_params = {{"image_type_ids", veci_image_type_id}}; + + return (RequestMethod( + str_sensor_topic_ + "/" + str_camera_id + "/GetImages", json_params, + pjson_out)); +} + +Status StaticSensorActor::Impl::RequestMethod(const std::string& str_method_path, + const json& json_params, + json* pjson_out) const { + Status status; + Message message_response; + + status = pclient_->Request(str_method_path, json_params, &message_response); + if (status == Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() == 0) + *pjson_out = response_message.GetResult(); + else { + status = Status::RejectedByServer; + log.ErrorF( + "Server method \"%s\" failed: error %d: %s", + str_method_path.c_str(), response_message.GetErrorCode(), + static_cast(response_message.GetResult()["message"]) + .c_str()); + } + } + + return (status); +} + +} // namespace client +} // namespace projectairsim +} // namespace microsoft diff --git a/client/cpp/AirSimClientDLL/src/Status.cpp b/client/cpp/ProjectAirsimClientLib/src/Status.cpp similarity index 100% rename from client/cpp/AirSimClientDLL/src/Status.cpp rename to client/cpp/ProjectAirsimClientLib/src/Status.cpp diff --git a/client/cpp/AirSimClientDLL/src/Topics.cpp b/client/cpp/ProjectAirsimClientLib/src/Topics.cpp similarity index 99% rename from client/cpp/AirSimClientDLL/src/Topics.cpp rename to client/cpp/ProjectAirsimClientLib/src/Topics.cpp index 3dd801d1..c6e59af6 100644 --- a/client/cpp/AirSimClientDLL/src/Topics.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/Topics.cpp @@ -7,7 +7,7 @@ #include -#include "AirSimClient.h" +#include "ProjectAirsimClient.h" #include "pch.h" using namespace std::chrono_literals; diff --git a/client/cpp/AirSimClientDLL/src/Topics.h b/client/cpp/ProjectAirsimClientLib/src/Topics.h similarity index 99% rename from client/cpp/AirSimClientDLL/src/Topics.h rename to client/cpp/ProjectAirsimClientLib/src/Topics.h index fd586fb8..6a5d8ea2 100644 --- a/client/cpp/AirSimClientDLL/src/Topics.h +++ b/client/cpp/ProjectAirsimClientLib/src/Topics.h @@ -4,6 +4,7 @@ // MIT License. All rights reserved. #pragma once +#include #include #include #include diff --git a/client/cpp/AirSimClientDLL/src/Types.cpp b/client/cpp/ProjectAirsimClientLib/src/Types.cpp similarity index 97% rename from client/cpp/AirSimClientDLL/src/Types.cpp rename to client/cpp/ProjectAirsimClientLib/src/Types.cpp index 73069420..3efe8a3f 100644 --- a/client/cpp/AirSimClientDLL/src/Types.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/Types.cpp @@ -37,6 +37,8 @@ ASC_DECL ColorRGBA::ColorRGBA(float red_in, float green_in, float blue_in, float alpha_in) : red(red_in), green(green_in), blue(blue_in), alpha(alpha_in) {} +ASC_DECL ReadyState::ReadyState(void) noexcept : is_ready(false), message() {} + ASC_DECL GeoPosition::GeoPosition(void) : altitude(0), latitude(0), longitude(0) {} diff --git a/client/cpp/AirSimClientDLL/src/Utils.cpp b/client/cpp/ProjectAirsimClientLib/src/Utils.cpp similarity index 90% rename from client/cpp/AirSimClientDLL/src/Utils.cpp rename to client/cpp/ProjectAirsimClientLib/src/Utils.cpp index 414e50a9..497b6ef9 100644 --- a/client/cpp/AirSimClientDLL/src/Utils.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/Utils.cpp @@ -5,12 +5,20 @@ #include "Utils.h" +#ifdef _WIN32 +#ifndef NOMINMAX +#define NOMINMAX +#endif +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif #include +#endif #include #include -#include "AirSimClient.h" +#include "ProjectAirsimClient.h" #include "core_sim/math_utils.hpp" #include "pch.h" @@ -162,6 +170,7 @@ VecStr SplitString(const std::string& str) { return (vec_str); } +#ifdef _WIN32 void UpdateClientVersion(HMODULE hmodule) { if (!v_str_version.empty()) return; @@ -173,23 +182,28 @@ void UpdateClientVersion(HMODULE hmodule) { wchar_t* wzProductVersion; // Get the version resource from the DLL module file - if ((GetModuleFileName(hmodule, wzPathModule, _countof(wzPathModule)) == 0) || - ((cbVersionInfo = GetFileVersionInfoSize(wzPathModule, 0)) == 0)) { + if ((GetModuleFileNameW(hmodule, wzPathModule, _countof(wzPathModule)) == 0) || + ((cbVersionInfo = GetFileVersionInfoSizeW(wzPathModule, nullptr)) == 0)) { return; } // Get the product version string vecVersionInfo.resize(cbVersionInfo); - if ((GetFileVersionInfo(wzPathModule, 0, (int)cbVersionInfo, - vecVersionInfo.data()) == 0) || - (VerQueryValue(vecVersionInfo.data(), - L"\\StringFileInfo\\040904B0\\ProductVersion", - (LPVOID*)&wzProductVersion, &cwch) == 0)) { + if ((GetFileVersionInfoW(wzPathModule, 0, (DWORD)cbVersionInfo, + vecVersionInfo.data()) == 0) || + (VerQueryValueW(vecVersionInfo.data(), + L"\\StringFileInfo\\040904B0\\ProductVersion", + (LPVOID*)&wzProductVersion, &cwch) == 0)) { return; } v_str_version = (wstring_convert.to_bytes(wzProductVersion).c_str()); } +#endif // _WIN32 + +#ifndef _WIN32 +void UpdateClientVersion(void* /*hmodule*/) {} +#endif void FromJSON(const json& json_in, Pose* ppose_out) { if (json_in.contains("position")) diff --git a/client/cpp/AirSimClientDLL/src/Utils.h b/client/cpp/ProjectAirsimClientLib/src/Utils.h similarity index 89% rename from client/cpp/AirSimClientDLL/src/Utils.h rename to client/cpp/ProjectAirsimClientLib/src/Utils.h index 00a5e52a..adc4cb3f 100644 --- a/client/cpp/AirSimClientDLL/src/Utils.h +++ b/client/cpp/ProjectAirsimClientLib/src/Utils.h @@ -4,13 +4,22 @@ // MIT License. All rights reserved. #pragma once +#ifdef _WIN32 +#ifndef NOMINMAX +#define NOMINMAX +#endif +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif #include +#endif #include #include #include "Common.h" #include "Status.h" +#include "Types.h" #include "json.hpp" using json = nlohmann::json; @@ -39,7 +48,11 @@ std::vector RPYToQuaternionCoordinates(float roll, float pitch, VecStr SplitString(const std::string& str); // Update the client libray version from the version resource +#ifdef _WIN32 void UpdateClientVersion(HMODULE hmodule); +#else +void UpdateClientVersion(void* hmodule); +#endif // Conversion from and to JSON void FromJSON(const json& json_in, Pose* ppose_out); diff --git a/client/cpp/AirSimClientDLL/src/World.cpp b/client/cpp/ProjectAirsimClientLib/src/World.cpp similarity index 63% rename from client/cpp/AirSimClientDLL/src/World.cpp rename to client/cpp/ProjectAirsimClientLib/src/World.cpp index 909e1643..26f6f88f 100644 --- a/client/cpp/AirSimClientDLL/src/World.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/World.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -17,11 +18,12 @@ #include #include #include +#include #include -#include "AirSimClient.h" -#include "AirSimMessage/jsonc.hpp" -#include "AirSimMessage/response_message.hpp" +#include "ProjectAirsimClient.h" +#include "ProjectAirSimMessage/jsonc.hpp" +#include "ProjectAirSimMessage/response_message.hpp" #include "Common.h" #include "JSONUtils.h" #include "RefCounted.h" @@ -31,7 +33,7 @@ #include "pch.h" namespace mp = microsoft::projectairsim; -namespace mpc = microsoft::projectairsim::client; +namespace pasc = microsoft::projectairsim::client; namespace microsoft { namespace projectairsim { @@ -78,7 +80,7 @@ class World::Impl { float delay_after_load_sec = 0, int sim_instance_idx = -1); public: - Status CreateVoxelGrid(const mpc::Pose& position, int x_size, int y_size, + Status CreateVoxelGrid(const pasc::Pose& position, int x_size, int y_size, int z_size, float resolution, BoolArray* pbool_array_ret, const std::vector& actors_to_ignore, @@ -88,6 +90,17 @@ class World::Impl { const VecStr& GetDrones(void) const; const char* GetParentTopic(void) const; Status HitTest(const Pose& pose, Vector3* pvec3_out); + Status ImportNEDTrajectory(const std::string& traj_name, + const std::vector& time, + const std::vector& pose_x, + const std::vector& pose_y, + const std::vector& pose_z, + const std::vector& pose_roll, + const std::vector& pose_pitch, + const std::vector& pose_yaw, + const std::vector& vel_lin_x, + const std::vector& vel_lin_y, + const std::vector& vel_lin_z); Status LoadScene(const json& json_scene_config, float delay_after_load_sec = 0); Status PlotDebugArrows(const VecVector3& points_start, @@ -109,6 +122,12 @@ class World::Impl { Status PlotDebugTransforms(const VecPose& poses, float scale, float thickness, float sec_duration, bool is_persistent); Status Resume(Message* pmessage_response_out); + Status RequestWorldJson(const std::string& method, + const json& json_params, + json* pjson_result_out) const; + Status RequestWorldBool(const std::string& method, + const json& json_params, + bool* pf_result_out) const; protected: static Status MakeAbsoluteSimConfigPath( @@ -165,7 +184,7 @@ ASC_DECL World::World(void) noexcept : pimpl_(new Impl()) {} ASC_DECL World::~World() {} ASC_DECL Status -World::CreateVoxelGrid(const mpc::Pose& position, int x_size, int y_size, +World::CreateVoxelGrid(const pasc::Pose& position, int x_size, int y_size, int z_size, float resolution, BoolArray* pbool_array_ret, const std::vector& actors_to_ignore, bool write_file, std::string file_path) const { @@ -190,10 +209,478 @@ ASC_DECL const char* World::GetParentTopic(void) const { return (pimpl_->GetParentTopic()); } +ASC_DECL Status World::GetSimClockType(std::string* pstr_clock_type_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson("GetSimClockType", json::object(), + &result)); + if (pstr_clock_type_out != nullptr) *pstr_clock_type_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::GetSimTime(std::int64_t* ptime_nanos_out) const { + json result; + RETURN_CATCH_STATUS( + pimpl_->RequestWorldJson("GetSimTime", json::object(), &result)); + if (ptime_nanos_out != nullptr) *ptime_nanos_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::Pause(std::string* pstr_state_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "Pause", json{{"do_pause", true}}, &result)); + if (pstr_state_out != nullptr) *pstr_state_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::Resume(std::string* pstr_state_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "Pause", json{{"do_pause", false}}, &result)); + if (pstr_state_out != nullptr) *pstr_state_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::IsPaused(bool* pf_is_paused_out) const { + RETURN_CATCH_STATUS( + pimpl_->RequestWorldBool("IsPaused", json::object(), pf_is_paused_out)); +} + +ASC_DECL Status World::ContinueForSimTime(std::int64_t delta_time_nanos, + bool wait_until_complete, + std::int64_t* ptime_nanos_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "ContinueForSimTime", + json{{"delta_time", delta_time_nanos}, + {"wait_until_complete", wait_until_complete}}, + &result)); + if (ptime_nanos_out != nullptr) *ptime_nanos_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::ContinueUntilSimTime(std::int64_t target_time_nanos, + bool wait_until_complete, + std::int64_t* ptime_nanos_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "ContinueUntilSimTime", + json{{"target_time", target_time_nanos}, + {"wait_until_complete", wait_until_complete}}, + &result)); + if (ptime_nanos_out != nullptr) *ptime_nanos_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::ContinueForNSteps(int n_steps, bool wait_until_complete, + std::int64_t* ptime_nanos_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "ContinueForNSteps", + json{{"n_steps", n_steps}, {"wait_until_complete", wait_until_complete}}, + &result)); + if (ptime_nanos_out != nullptr) *ptime_nanos_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::ContinueForSingleStep(bool wait_until_complete, + std::int64_t* ptime_nanos_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "ContinueForSingleStep", + json{{"wait_until_complete", wait_until_complete}}, &result)); + if (ptime_nanos_out != nullptr) *ptime_nanos_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::SetSunlightIntensity(float intensity, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetSunLightIntensity", json{{"intensity", intensity}}, pf_success_out)); +} + +ASC_DECL Status World::GetSunlightIntensity(float* pintensity_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson("GetSunLightIntensity", + json::object(), &result)); + if (pintensity_out != nullptr) *pintensity_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::SetCloudShadowStrength(float strength, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetCloudShadowStrength", json{{"strength", strength}}, pf_success_out)); +} + +ASC_DECL Status World::GetCloudShadowStrength(float* pstrength_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson("GetCloudShadowStrength", + json::object(), &result)); + if (pstrength_out != nullptr) *pstrength_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::SetWindVelocity(float v_x, float v_y, float v_z, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetWindVelocity", json{{"v_x", v_x}, {"v_y", v_y}, {"v_z", v_z}}, + pf_success_out)); +} + +ASC_DECL Status World::GetWindVelocity(Vector3* pwind_velocity_out) const { + json result; + RETURN_CATCH_STATUS( + pimpl_->RequestWorldJson("GetWindVelocity", json::object(), &result)); + if (pwind_velocity_out != nullptr && result.is_array() && result.size() >= 3) { + pwind_velocity_out->x = result[0].get(); + pwind_velocity_out->y = result[1].get(); + pwind_velocity_out->z = result[2].get(); + } + return (Status::OK); +} + +ASC_DECL Status World::ListActors(VecStr* pvecstr_actor_names_out) const { + json result; + RETURN_CATCH_STATUS( + pimpl_->RequestWorldJson("ListActors", json::object(), &result)); + if (pvecstr_actor_names_out != nullptr) { + *pvecstr_actor_names_out = result.get(); + } + return (Status::OK); +} + +ASC_DECL Status World::ListObjects(const std::string& name_regex, + VecStr* pvecstr_object_names_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "ListObjects", json{{"name", name_regex}}, &result)); + if (pvecstr_object_names_out != nullptr) { + *pvecstr_object_names_out = result.get(); + } + return (Status::OK); +} + +ASC_DECL Status World::ListAssets(const std::string& name_regex, + VecStr* pvecstr_asset_names_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "ListAssets", json{{"name", name_regex}}, &result)); + if (pvecstr_asset_names_out != nullptr) { + *pvecstr_asset_names_out = result.get(); + } + return (Status::OK); +} + +ASC_DECL Status World::GetObjectPose(const std::string& object_name, + json* pjson_pose_out) const { + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "GetObjectPose", json{{"object_name", object_name}}, pjson_pose_out)); +} + +ASC_DECL Status World::GetObjectPoses(const VecStr& object_names, + json* pjson_poses_out) const { + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "GetObjectPoses", json{{"object_names", object_names}}, pjson_poses_out)); +} + +ASC_DECL Status World::SetObjectPose(const std::string& object_name, + const json& json_pose, bool teleport, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetObjectPose", + json{{"object_name", object_name}, {"pose", json_pose}, {"teleport", teleport}}, + pf_success_out)); +} + +ASC_DECL Status World::GetObjectScale( + const std::string& object_name, std::vector* pvec_scale_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "GetObjectScale", json{{"object_name", object_name}}, &result)); + if (pvec_scale_out != nullptr) *pvec_scale_out = result.get>(); + return (Status::OK); +} + +ASC_DECL Status World::SetObjectScale(const std::string& object_name, + const std::vector& scale, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetObjectScale", json{{"object_name", object_name}, {"scale", scale}}, + pf_success_out)); +} + +ASC_DECL Status World::SpawnObject(const std::string& object_name, + const std::string& asset_path, + const json& json_pose, + const std::vector& scale, + bool enable_physics, + std::string* pstr_spawned_name_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "SpawnObject", + json{{"object_name", object_name}, + {"asset_path", asset_path}, + {"pose", json_pose}, + {"scale", scale}, + {"enable_physics", enable_physics}}, + &result)); + if (pstr_spawned_name_out != nullptr) *pstr_spawned_name_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::SpawnObjectFromFile( + const std::string& object_name, const std::string& file_format, + const VecB& asset_bytes, bool is_binary, const json& json_pose, + const std::vector& scale, bool enable_physics, + std::string* pstr_spawned_name_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "spawnObjectFromFile", + json{{"object_name", object_name}, + {"file_format", file_format}, + {"byte_array", asset_bytes}, + {"is_binary", is_binary}, + {"pose", json_pose}, + {"scale", scale}, + {"enable_physics", enable_physics}}, + &result)); + if (pstr_spawned_name_out != nullptr) *pstr_spawned_name_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::SpawnObjectAtGeo( + const std::string& object_name, const std::string& asset_path, + float latitude, float longitude, float altitude, + const std::vector& rotation, const std::vector& scale, + bool enable_physics, std::string* pstr_spawned_name_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "spawnObjectAtGeo", + json{{"object_name", object_name}, + {"asset_path", asset_path}, + {"latitude", latitude}, + {"longitude", longitude}, + {"altitude", altitude}, + {"rotation", rotation}, + {"scale", scale}, + {"enable_physics", enable_physics}}, + &result)); + if (pstr_spawned_name_out != nullptr) *pstr_spawned_name_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::SpawnObjectFromFileAtGeo( + const std::string& object_name, const std::string& file_format, + const VecB& asset_bytes, bool is_binary, float latitude, float longitude, + float altitude, const std::vector& rotation, + const std::vector& scale, bool enable_physics, + std::string* pstr_spawned_name_out) { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "spawnObjectFromFileAtGeo", + json{{"object_name", object_name}, + {"file_format", file_format}, + {"byte_array", asset_bytes}, + {"is_binary", is_binary}, + {"latitude", latitude}, + {"longitude", longitude}, + {"altitude", altitude}, + {"rotation", rotation}, + {"scale", scale}, + {"enable_physics", enable_physics}}, + &result)); + if (pstr_spawned_name_out != nullptr) *pstr_spawned_name_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::DestroyObject(const std::string& object_name, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "DestroyObject", json{{"object_name", object_name}}, pf_success_out)); +} + +ASC_DECL Status World::DestroyAllSpawnedObjects(bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "DestroyAllSpawnedObjects", json::object(), pf_success_out)); +} + +ASC_DECL Status World::EnableWeatherVisualEffects(bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SimSetWeatherVisualEffectsStatus", json{{"status", true}}, pf_success_out)); +} + +ASC_DECL Status World::DisableWeatherVisualEffects(bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SimSetWeatherVisualEffectsStatus", json{{"status", false}}, pf_success_out)); +} + +ASC_DECL Status World::ResetWeatherEffects(bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "ResetWeatherEffects", json::object(), pf_success_out)); +} + +ASC_DECL Status World::SetWeatherVisualEffectsParam(int param, float value, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetWeatherVisualEffectsParameter", + json{{"param", param}, {"value", value}}, pf_success_out)); +} + +ASC_DECL Status World::GetWeatherVisualEffectsParam( + json* pjson_params_out) const { + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "GetWeatherVisualEffectsParameter", json::object(), pjson_params_out)); +} + +ASC_DECL Status World::SetObjectMaterial( + const std::string& object_name, const std::string& material_asset_path, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetObjectMaterial", + json{{"object_name", object_name}, + {"material_asset_path", material_asset_path}}, + pf_success_out)); +} + +ASC_DECL Status World::SetObjectTextureFromUrl(const std::string& object_name, + const std::string& url, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetObjectTextureFromUrl", + json{{"object_name", object_name}, {"url", url}}, pf_success_out)); +} + +ASC_DECL Status World::SetObjectTextureFromFile( + const std::string& object_name, const std::string& texture_file_path, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetObjectTextureFromFile", + json{{"object_name", object_name}, {"texture_file_path", texture_file_path}}, + pf_success_out)); +} + +ASC_DECL Status World::SetObjectTextureFromPackagedAsset( + const std::string& object_name, const std::string& texture_asset_path, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetObjectTextureFromPackagedAsset", + json{{"object_name", object_name}, {"texture_asset_path", texture_asset_path}}, + pf_success_out)); +} + +ASC_DECL Status World::SwapObjectTexture(const std::string& tag, int tex_id, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SwapObjectTexture", json{{"tag", tag}, {"tex_id", tex_id}}, + pf_success_out)); +} + +ASC_DECL Status World::SetLightObjectIntensity( + const std::string& object_name, float new_intensity, bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetLightObjectIntensity", + json{{"object_name", object_name}, {"new_intensity", new_intensity}}, + pf_success_out)); +} + +ASC_DECL Status World::SetLightObjectColor( + const std::string& object_name, const std::vector& color_rgb, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetLightObjectColor", + json{{"object_name", object_name}, {"color_rgb", color_rgb}}, + pf_success_out)); +} + +ASC_DECL Status World::SetLightObjectRadius(const std::string& object_name, + float new_radius, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetLightObjectRadius", + json{{"object_name", object_name}, {"new_radius", new_radius}}, + pf_success_out)); +} + +ASC_DECL Status World::SetTimeOfDay(bool enabled, const std::string& datetime, + bool is_dst, float clock_speed, + float update_interval, bool move_sun, + bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetTimeOfDay", + json{{"status", enabled}, + {"datetime", datetime}, + {"is_dst", is_dst}, + {"clock_speed", clock_speed}, + {"update_interval", update_interval}, + {"move_sun", move_sun}}, + pf_success_out)); +} + +ASC_DECL Status World::GetTimeOfDay(json* pjson_time_of_day_out) const { + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson("GetTimeOfDay", json::object(), + pjson_time_of_day_out)); +} + +ASC_DECL Status World::SetSunPositionFromDateTime( + const std::string& date_time, bool is_dst, bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetSunPositionFromDateTime", + json{{"date_time", date_time}, {"is_dst", is_dst}}, pf_success_out)); +} + +ASC_DECL Status World::SwitchStreamingView(bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SwitchStreamingView", json::object(), pf_success_out)); +} + +ASC_DECL Status World::SetSegmentationIDByName( + const std::string& mesh_name, int segmentation_id, bool is_name_regex, + bool use_owner_name, bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetSegmentationIDByName", + json{{"mesh_name", mesh_name}, + {"segmentation_id", segmentation_id}, + {"is_name_regex", is_name_regex}, + {"use_owner_name", use_owner_name}}, + pf_success_out)); +} + +ASC_DECL Status World::GetSegmentationIDByName( + const std::string& mesh_name, bool use_owner_name, + int* psegmentation_id_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "GetSegmentationIDByName", + json{{"mesh_name", mesh_name}, {"use_owner_name", use_owner_name}}, + &result)); + if (psegmentation_id_out != nullptr) *psegmentation_id_out = result.get(); + return (Status::OK); +} + +ASC_DECL Status World::GetSegmentationIDMap( + json* pjson_segmentation_map_out) const { + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson("GetSegmentationIDMap", + json::object(), + pjson_segmentation_map_out)); +} + ASC_DECL Status World::HitTest(const Pose& pose, Vector3* pvec3_out) { return (pimpl_->HitTest(pose, pvec3_out)); } +ASC_DECL Status World::ImportNEDTrajectory( + const std::string& traj_name, const std::vector& time, + const std::vector& pose_x, const std::vector& pose_y, + const std::vector& pose_z, const std::vector& pose_roll, + const std::vector& pose_pitch, const std::vector& pose_yaw, + const std::vector& vel_lin_x, const std::vector& vel_lin_y, + const std::vector& vel_lin_z) { + RETURN_CATCH_STATUS(pimpl_->ImportNEDTrajectory( + traj_name, time, pose_x, pose_y, pose_z, pose_roll, pose_pitch, pose_yaw, + vel_lin_x, vel_lin_y, vel_lin_z)); +} + ASC_DECL Status World::Initialize(std::shared_ptr& pclient, const std::string& scene_config_name, const std::string& sim_config_path, @@ -253,6 +740,75 @@ ASC_DECL Status World::PlotDebugTransforms(const VecPose& poses, float scale, sec_duration, is_persistent)); } +ASC_DECL Status World::PlotDebugTransformsWithNames( + const VecPose& poses, const VecStr& names, float tf_scale, + float tf_thickness, float text_scale, const ColorRGBA& text_color_rgba, + float sec_duration) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "debugPlotTransformsWithNames", + json{{"poses", poses}, + {"names", names}, + {"tf_scale", tf_scale}, + {"tf_thickness", tf_thickness}, + {"text_scale", text_scale}, + {"text_color_rgba", text_color_rgba}, + {"duration", sec_duration}}, + nullptr)); +} + +ASC_DECL Status World::SetTraceLine(const ColorRGBA& color_rgba, + float thickness, bool* pf_success_out) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "SetTraceLine", json{{"color_rgba", color_rgba}, {"thickness", thickness}}, + pf_success_out)); +} + +ASC_DECL Status World::ToggleTrace(bool* pf_success_out) { + RETURN_CATCH_STATUS( + pimpl_->RequestWorldBool("ToggleTrace", json::object(), pf_success_out)); +} + +ASC_DECL Status World::Get3DBoundingBox( + const std::string& object_name, int box_alignment, + json* pjson_bounding_box_out) const { + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "Get3DBoundingBox", + json{{"object_name", object_name}, {"box_alignment", box_alignment}}, + pjson_bounding_box_out)); +} + +ASC_DECL Status World::ImportGeoTrajectory( + const std::string& traj_name, const std::vector& time, + const std::vector& latitudes, const std::vector& longitudes, + const std::vector& altitudes, const std::vector& roll, + const std::vector& pitch, const std::vector& yaw, + const std::vector& vel_lin_x, const std::vector& vel_lin_y, + const std::vector& vel_lin_z) { + RETURN_CATCH_STATUS(pimpl_->RequestWorldBool( + "ImportGeoTrajectory", + json{{"traj_name", traj_name}, + {"time", time}, + {"latitudes", latitudes}, + {"longitudes", longitudes}, + {"altitudes", altitudes}, + {"roll", roll}, + {"pitch", pitch}, + {"yaw", yaw}, + {"vel_lin_x", vel_lin_x}, + {"vel_lin_y", vel_lin_y}, + {"vel_lin_z", vel_lin_z}}, + nullptr)); +} + +ASC_DECL Status World::GetSurfaceElevationAtPoint(float x, float y, + float* pelevation_out) const { + json result; + RETURN_CATCH_STATUS(pimpl_->RequestWorldJson( + "GetSurfaceElevationAtPoint", json{{"x", x}, {"y", y}}, &result)); + if (pelevation_out != nullptr) *pelevation_out = result.get(); + return (Status::OK); +} + World::Impl::Impl(void) noexcept : file_paths_(), geo_position_home_(), @@ -263,6 +819,53 @@ World::Impl::Impl(void) noexcept World::Impl::~Impl() {} +Status World::Impl::RequestWorldJson(const std::string& method, + const json& json_params, + json* pjson_result_out) const { + Message message_response; + Status status; + + if ((status = pclient_->Request(str_parent_topic_ + "/" + method, + json_params, &message_response)) == + Status::OK) { + ResponseMessage response_message; + + response_message.Deserialize(message_response); + if (response_message.GetErrorCode() != 0) { + json json_result = response_message.GetResult(); + + if (json_result.contains("message")) + log.ErrorF("Server method \"%s/%s\" failed: error %d: %s", + str_parent_topic_.c_str(), method.c_str(), + response_message.GetErrorCode(), + static_cast(json_result["message"]).c_str()); + else + log.ErrorF("Server method \"%s/%s\" failed: error %d", + str_parent_topic_.c_str(), method.c_str(), + response_message.GetErrorCode()); + + return (Status::RejectedByServer); + } + + if (pjson_result_out != nullptr) + *pjson_result_out = response_message.GetResult(); + } + + return (status); +} + +Status World::Impl::RequestWorldBool(const std::string& method, + const json& json_params, + bool* pf_result_out) const { + json result; + Status status = RequestWorldJson(method, json_params, &result); + + if (status == Status::OK && pf_result_out != nullptr) + *pf_result_out = result.is_boolean() && static_cast(result); + + return (status); +} + Status World::Impl::CreateVoxelGrid( const Pose& position, int x_size, int y_size, int z_size, float resolution, BoolArray* pbool_array_ret, @@ -463,6 +1066,80 @@ Status World::Impl::HitTest(const Pose& pose, Vector3* pvec3_out) { return (status); } +Status World::Impl::ImportNEDTrajectory( + const std::string& traj_name, const std::vector& time, + const std::vector& pose_x, const std::vector& pose_y, + const std::vector& pose_z, const std::vector& pose_roll, + const std::vector& pose_pitch, const std::vector& pose_yaw, + const std::vector& vel_lin_x, const std::vector& vel_lin_y, + const std::vector& vel_lin_z) { + const size_t point_count = time.size(); + if (point_count == 0 || pose_x.size() != point_count || + pose_y.size() != point_count || pose_z.size() != point_count) { + return Status::Failed; + } + + auto make_zero_vector = [point_count]() { + return std::vector(point_count, 0.0f); + }; + + std::vector roll = pose_roll.empty() ? make_zero_vector() : pose_roll; + std::vector pitch = pose_pitch; + std::vector yaw = pose_yaw; + std::vector vel_x = vel_lin_x.empty() ? make_zero_vector() : vel_lin_x; + std::vector vel_y = vel_lin_y.empty() ? make_zero_vector() : vel_lin_y; + std::vector vel_z = vel_lin_z.empty() ? make_zero_vector() : vel_lin_z; + + if (pitch.empty()) { + pitch.reserve(point_count); + for (size_t idx = 1; idx < point_count; ++idx) { + const float dx = pose_x[idx - 1] - pose_x[idx]; + const float dy = pose_y[idx - 1] - pose_y[idx]; + const float dz = pose_z[idx - 1] - pose_z[idx]; + pitch.emplace_back(std::atan2(-dz, std::hypot(dx, dy))); + } + pitch.emplace_back(pitch.empty() ? 0.0f : pitch.back()); + } + + if (yaw.empty()) { + yaw.reserve(point_count); + for (size_t idx = 1; idx < point_count; ++idx) { + const float dx = pose_x[idx - 1] - pose_x[idx]; + const float dy = pose_y[idx - 1] - pose_y[idx]; + yaw.emplace_back(std::atan2(dy, dx)); + } + yaw.emplace_back(yaw.empty() ? 0.0f : yaw.back()); + } + + if (roll.size() != point_count || pitch.size() != point_count || + yaw.size() != point_count || vel_x.size() != point_count || + vel_y.size() != point_count || vel_z.size() != point_count) { + return Status::Failed; + } + + const json json_params = { + {"traj_name", traj_name}, {"time", time}, {"pose_x", pose_x}, + {"pose_y", pose_y}, {"pose_z", pose_z}, {"pose_roll", roll}, + {"pose_pitch", pitch}, {"pose_yaw", yaw}, {"vel_x_lin", vel_x}, + {"vel_y_lin", vel_y}, {"vel_z_lin", vel_z}, + }; + + Message message; + Status status = + pclient_->Request(str_parent_topic_ + "/ImportNEDTrajectory", + json_params, &message); + if (status != Status::OK) return status; + + ResponseMessage responsemessage; + responsemessage.Deserialize(message); + if (responsemessage.GetErrorCode() != 0 || + !static_cast(responsemessage.GetResult())) { + return Status::RejectedByServer; + } + + return Status::OK; +} + Status World::Impl::Initialize(std::shared_ptr& pclient, const std::string& scene_config_name, const std::string& sim_config_path, @@ -861,7 +1538,16 @@ Status World::Impl::LoadObjectsViaFileFromConfig( FILE* pfile = fopen(str_asset_path.c_str(), "rb"); struct stat stat_asset = {0}; + if (pfile == nullptr) { + status = Status::NotFound; + return (status); + } + + #ifdef _WIN32 fstat(_fileno(pfile), &stat_asset); + #else + fstat(fileno(pfile), &stat_asset); + #endif vecb_asset.resize(stat_asset.st_size); cb = fread(vecb_asset.data(), 1, vecb_asset.size(), pfile); if (cb != stat_asset.st_size) vecb_asset.resize(cb); diff --git a/client/cpp/AirSimClientDLL/src/airsim_proxies/math_utils.cpp b/client/cpp/ProjectAirsimClientLib/src/airsim_proxies/math_utils.cpp similarity index 76% rename from client/cpp/AirSimClientDLL/src/airsim_proxies/math_utils.cpp rename to client/cpp/ProjectAirsimClientLib/src/airsim_proxies/math_utils.cpp index 5411ad56..1c55543f 100644 --- a/client/cpp/AirSimClientDLL/src/airsim_proxies/math_utils.cpp +++ b/client/cpp/ProjectAirsimClientLib/src/airsim_proxies/math_utils.cpp @@ -3,4 +3,4 @@ // // MIT License. All rights reserved. -#include +#include diff --git a/client/cpp/README.md b/client/cpp/README.md new file mode 100644 index 00000000..b9230fff --- /dev/null +++ b/client/cpp/README.md @@ -0,0 +1,264 @@ +# ProjectAirSim C++ Client + +Native C++ client library for interacting with the ProjectAirSim simulation server. + +## C++ Client Assets + +- Documentation: `client/cpp/docs/` +- Scenario scripts: `client/cpp/scripts/` +- Scenario app: `client/cpp/example_user_apps/CppClientScenarios/` +- User scenario template app: `client/cpp/example_user_apps/UserScenarioTemplate/` + +Primary docs entrypoint: `client/cpp/docs/INDEX.md`. + +Preferred end-user flow: build and run a single custom scenario app (instead of running all predefined scenarios). See `client/cpp/docs/README_CPP_CLIENT_SCENARIOS.md` and `client/cpp/scripts/run_cpp_user_scenario.sh`. + +## Overview + +The C++ client provides the same simulation control capabilities as the Python client but as a set of linkable static libraries, suitable for performance-critical applications or for projects that cannot use Python. + +### Libraries + +| Library | Description | +|---|---| +| `NNGI` | NNG-based IPC/TCP transport wrapper | +| `ProjectAirSimMessageLib` | Message serialization (msgpack + JSON) | +| `ProjectAirsimClient` | High-level simulation API (Client, World, Drone) | + +### Public API Headers (`ProjectAirsimClientLib/Include/ProjectAirsimClient/`) + +| Header | Contents | +|---|---| +| `ProjectAirsimClient.h` | Main include — pulls in all other headers | +| `Client.h` | `Client` — connects to the simulation server | +| `World.h` | `World` — loads scenes, controls physics step | +| `Drone.h` | `Drone` — takeoff, move, land, arm/disarm | +| `AsyncResult.h` | `AsyncResult` — non-blocking operation handles | +| `Status.h` | `Status` enum and `GetStatusString()` | +| `Types.h` | `LandedState`, `ReadyState`, vectors, poses | +| `Log.h` | Logging with configurable sink | + +All public types live in the `microsoft::projectairsim::client` namespace (alias: `pasc`). + +--- + +## Building + +### Prerequisites + +- CMake ≥ 3.20 +- C++17 compiler (GCC ≥ 10 or Clang ≥ 13) +- Internet access on first build (FetchContent downloads NNG, nlohmann-json, msgpack, optionally Eigen3) + +System Eigen3 is used if available (`sudo apt install libeigen3-dev`), otherwise it is fetched automatically. + +### Via `build.sh` (recommended) + +The C++ client is built automatically as part of the simulation library build steps: + +```bash +# Debug build (sim libs + C++ client) +./build.sh simlibs_debug + +# Release build (sim libs + C++ client) +./build.sh simlibs_release + +# C++ client only (skips sim libs) +./build.sh cpp_client_debug +./build.sh cpp_client_release +``` + +Binaries are placed in: +- `client/cpp/build_linux/Debug/hello_drone` +- `client/cpp/build_linux/Release/hello_drone` + +### Via `build.cmd` on Windows + +Windows builds reuse the existing `ProjectAirsimClientLib` and `HelloDrone` solution artifacts: + +```bat +build.cmd cpp_client_debug +build.cmd cpp_client_release +``` + +Outputs are placed in the existing project locations: +- `client/cpp/libraries/x64/Debug` +- `client/cpp/libraries/x64/Release` +- `client/cpp/example_user_apps/HelloDrone/x64/Debug` +- `client/cpp/example_user_apps/HelloDrone/x64/Release` + +### Manual CMake build + +```bash +cd client/cpp + +# Configure +cmake -S . -B build_linux/Debug -DCMAKE_BUILD_TYPE=Debug +cmake -S . -B build_linux/Release -DCMAKE_BUILD_TYPE=Release + +# Build +cmake --build build_linux/Debug -j$(nproc) +cmake --build build_linux/Release -j$(nproc) +``` + +### Cleaning + +```bash +# Via build.sh (also cleans sim libs) +./build.sh clean + +# Manual +rm -rf client/cpp/build_linux +``` + +--- + +## Running the Example: HelloDrone + +`hello_drone` performs a minimal flight sequence: **arm → takeoff → move up 1 m/s for 4 s → land → disarm**. + +### Requirements + +- Simulation server must be running (Unreal Editor with ProjectAirSim plugin, or packaged Blocks environment) +- A `sim_config/` directory with scene configuration files (`.jsonc`) + +### Command-line options + +``` +hello_drone [--simhost host_or_ip] [--simconfig sim_config_path] + + --simhost host_or_ip Hostname or IP of the simulation server (default: "localhost") + --simconfig sim_config_path Path to the sim_config directory (default: "sim_config/") +``` + +### Example + +```bash +cd client/cpp/build_linux/Debug + +./hello_drone \ + --simhost 127.0.0.1 \ + --simconfig ../../../../client/python/example_user_scripts/sim_config/ +``` + +The sim_config directory must contain a `scene_basic_drone.jsonc` (or equivalent scene file matching your Unreal environment setup). Example configs are in `client/python/example_user_scripts/sim_config/`. + +### Expected output + +``` +Looking for scene file "sim_config/scene_basic_drone.jsonc" +Ready state: ready (...) +TakeoffAsync: starting +TakeoffAsync: completed +Move-Up invoked +Move-Up completed +LandAsync: starting +Landed state: landed +LandAsync: completed +Drone landed. +``` + +--- + +## Writing Your Own App + +### 1. Minimal CMakeLists.txt + +```cmake +cmake_minimum_required(VERSION 3.20) +project(MyDroneApp LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) + +# Point to the pre-built client libraries +set(CPP_CLIENT_ROOT "/path/to/ProjectAirSim/client/cpp") +set(PROJECTAIRSIM_ROOT "/path/to/ProjectAirSim") + +# Import the static libraries (already built via build.sh) +add_library(ProjectAirsimClient STATIC IMPORTED) +set_target_properties(ProjectAirsimClient PROPERTIES + IMPORTED_LOCATION "${CPP_CLIENT_ROOT}/build_linux/Release/libProjectAirsimClient.a" + INTERFACE_INCLUDE_DIRECTORIES + "${CPP_CLIENT_ROOT}/ProjectAirsimClientLib/Include;${PROJECTAIRSIM_ROOT}/core_sim/include" +) + +add_executable(my_app main.cpp) +target_link_libraries(my_app PRIVATE ProjectAirsimClient pthread) +``` + +> **Tip**: The simplest approach is to add your source files directly to `client/cpp/CMakeLists.txt` alongside `hello_drone` and rebuild via `build.sh cpp_client_debug`. + +### 2. Minimal app skeleton + +```cpp +#include +#include + +namespace pasc = microsoft::projectairsim::client; + +int main() { + // Optional: redirect logs to stdout + pasc::log.SetLogSink([](pasc::Log::Severity, const char* msg) noexcept { + std::cout << msg << std::endl; + }); + + // Connect to the running simulation server + auto client = std::make_shared(); + if (client->Connect("localhost") != pasc::Status::OK) return 1; + + // Load scene and initialize world + auto world = std::make_shared(); + if (world->Initialize(client, "sim_config/scene_basic_drone.jsonc", + "sim_config/", 2.0f) != pasc::Status::OK) return 1; + + // Connect to a drone defined in the scene + auto drone = std::make_shared(); + if (drone->Initialize(client, world, "Drone1") != pasc::Status::OK) return 1; + + // Arm and enable API control + bool ok; + drone->EnableAPIControl(&ok); + drone->Arm(&ok); + + // Fly! + drone->TakeoffAsync().Wait(); + drone->MoveByVelocityAsync(0.0f, 0.0f, -1.0f, 3.0).Wait(); // up 1 m/s for 3 s + drone->LandAsync().Wait(); + + drone->Disarm(&ok); + drone->DisableAPIControl(&ok); + client->Disconnect(); + return 0; +} +``` + +### 3. Async operations + +All movement commands return an `AsyncResult`. Two usage patterns: + +```cpp +// Pattern 1: blocking wait +auto result = drone->TakeoffAsync(); +pasc::Status status = result.Wait(); // blocks until done + +// Pattern 2: poll + wait +auto result = drone->MoveByVelocityAsync(0, 0, -1, 5.0); +while (!result.FIsDone()) + std::this_thread::sleep_for(std::chrono::milliseconds(5)); +status = result.Wait(); // must still call Wait() after FIsDone() returns true +``` + +--- + +## Architecture + +``` +hello_drone (executable) + └── ProjectAirsimClient (static lib) + ├── ProjectAirSimMessageLib (static lib) + │ └── nlohmann-json, msgpack, Eigen3 + └── NNGI (static lib) + └── nng (static lib, fetched via FetchContent) +``` + +All dependencies except `libeigen3-dev` (optional system package) are fetched automatically by CMake FetchContent on first configure. diff --git a/client/cpp/docs/README_CPP_CLIENT_SCENARIOS.md b/client/cpp/docs/README_CPP_CLIENT_SCENARIOS.md new file mode 100644 index 00000000..f3f8b7c9 --- /dev/null +++ b/client/cpp/docs/README_CPP_CLIENT_SCENARIOS.md @@ -0,0 +1,138 @@ +# ProjectAirSim C++ User Scenario Apps + +This document describes the recommended workflow for end users: create or customize a single C++ app for your scenario and run only that app. + +## Scope and Layout + +Following the same organization principle used by the Python client (client-specific assets under `client/python/...`), all C++ client scenario assets are under `client/cpp/...`: + +- Scripts: `client/cpp/scripts/` +- Docs: `client/cpp/docs/` +- Scenario app source: `client/cpp/example_user_apps/CppClientScenarios/` +- User app template source: `client/cpp/example_user_apps/UserScenarioTemplate/` +- Built binary: `client/cpp/build_local/cpp_client_scenarios` +- Built user-template binary: `client/cpp/build_local/user_scenario_template` + +## Prerequisites + +1. Simulation server running (default host `127.0.0.1`). +2. C++ client built in `client/cpp/build_local`. +3. Scene configs available in `client/python/example_user_scripts/sim_config/`. + +## Build + +```bash +cmake -S client/cpp -B client/cpp/build_local -DCMAKE_BUILD_TYPE=Debug +cmake --build client/cpp/build_local --target cpp_client_scenarios -j$(nproc) +cmake --build client/cpp/build_local --target user_scenario_template -j$(nproc) +``` + +## Recommended: Run One User Scenario App + +Use the dedicated runner for single-app execution: + +```bash +./client/cpp/scripts/run_cpp_user_scenario.sh +``` + +Common options: + +```bash +./client/cpp/scripts/run_cpp_user_scenario.sh \ + --simhost 127.0.0.1 \ + --simconfig client/python/example_user_scripts/sim_config \ + --scene scene_basic_drone.jsonc \ + --vehicle Drone1 +``` + +If you create a custom executable target, pass it with `--target`: + +```bash +./client/cpp/scripts/run_cpp_user_scenario.sh --target my_custom_scenario_app +``` + +Rover-specific example: + +```bash +./client/cpp/scripts/run_cpp_user_scenario.sh \ + --target user_rover_scenario \ + --scene scene_basic_rover.jsonc \ + --vehicle Rover1 +``` + +## Creating Your Own Scenario App + +1. Copy `client/cpp/example_user_apps/UserScenarioTemplate/UserScenarioTemplate.cpp`. +2. Rename the copied file and add your own logic inside `RunUserScenario(...)`. +3. Add a new `add_executable(...)` entry to `client/cpp/CMakeLists.txt`. +4. Link your target to `ProjectAirsimClient` (and `Threads::Threads` on Linux). +5. Build and run with `run_cpp_user_scenario.sh --target `. + +## Legacy Multi-Scenario Script + +The old orchestrator script is still available: + +```bash +./client/cpp/scripts/run_cpp_client_scenarios.sh --only basic +``` + +It now requires `--only` and no longer runs all scenarios by default. + +## Legacy Scenario Wrappers + +```bash +./client/cpp/scripts/run_cpp_client_scenario_basic.sh +./client/cpp/scripts/run_cpp_client_scenario_sensors.sh +./client/cpp/scripts/run_cpp_client_scenario_two_drones.sh +./client/cpp/scripts/run_cpp_client_scenario_wind.sh +./client/cpp/scripts/run_cpp_client_scenario_battery.sh +``` + +## Scenario Coverage + +- `basic`: API control, arm/disarm, takeoff, move, land. +- `sensors`: IMU, GPS, barometer, magnetometer, airspeed, camera, battery. +- `two_drones`: two-vehicle initialization, movement, kinematics. +- `wind`: sunlight, cloud shadow, wind velocity, pause/resume, sim time. +- `battery`: battery state and battery drain-rate controls. + +## Scene Files + +All scenario configurations are in: + +`client/python/example_user_scripts/sim_config/` + +Key files: + +- `scene_basic_drone.jsonc` +- `scene_drone_sensors.jsonc` +- `scene_two_drones.jsonc` +- `scene_drone_wind.jsonc` +- `scene_battery_simple.jsonc` + +## Success Criteria + +- Exit code `0`: success. +- Exit code `1`: failure. +- Expected completion marker: + +```text +[PASS] User scenario completed +``` + +## Troubleshooting + +Connection issues: + +```bash +ss -tln | grep 7777 +./client/cpp/scripts/run_cpp_user_scenario.sh --simhost +``` + +Rebuild if binary is missing: + +```bash +rm -rf client/cpp/build_local +cmake -S client/cpp -B client/cpp/build_local -DCMAKE_BUILD_TYPE=Debug +cmake --build client/cpp/build_local --target cpp_client_scenarios -j$(nproc) +``` diff --git a/client/cpp/example_user_apps/CppClientScenarios/CppClientScenarios.cpp b/client/cpp/example_user_apps/CppClientScenarios/CppClientScenarios.cpp new file mode 100644 index 00000000..f95129d9 --- /dev/null +++ b/client/cpp/example_user_apps/CppClientScenarios/CppClientScenarios.cpp @@ -0,0 +1,374 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pasc = microsoft::projectairsim::client; +namespace mp = microsoft::projectairsim; +using pasc::Status; +using nlohmann::json; + +void LogSink(pasc::Log::Severity /*severity*/, const char* message) noexcept { + std::cout << message << std::endl; +} + +std::string StatusToString(Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +bool Ensure(Status status, const std::string& where) { + if (status == Status::OK) return true; + std::cerr << "[ERROR] " << where << ": " << StatusToString(status) << std::endl; + return false; +} + +bool RequestJson(std::shared_ptr& client, const std::string& method, + const json& params, json* result_out = nullptr) { + pasc::Message message_response; + Status status = client->Request(method, params, &message_response); + if (!Ensure(status, method)) return false; + + mp::ResponseMessage response; + response.Deserialize(message_response); + + if (response.GetErrorCode() != 0) { + std::cerr << "[ERROR] " << method << " rejected by server: " + << response.GetResult().dump() << std::endl; + return false; + } + + if (result_out != nullptr) *result_out = response.GetResult(); + return true; +} + +bool EnableArmTakeoff(std::shared_ptr& drone, const std::string& name) { + bool state = false; + if (!Ensure(drone->EnableAPIControl(&state), name + " EnableAPIControl")) return false; + if (!Ensure(drone->Arm(&state), name + " Arm")) return false; + auto ar = drone->TakeoffAsync(); + if (!Ensure(ar.Wait(), name + " TakeoffAsync")) return false; + return true; +} + +bool LandDisarm(std::shared_ptr& drone, const std::string& name) { + bool state = false; + auto ar = drone->LandAsync(); + if (!Ensure(ar.Wait(), name + " LandAsync")) return false; + if (!Ensure(drone->Disarm(&state), name + " Disarm")) return false; + if (!Ensure(drone->DisableAPIControl(&state), name + " DisableAPIControl")) return false; + return true; +} + +bool InitDrone(std::shared_ptr& client, std::shared_ptr& world, + const std::string& name, std::shared_ptr* drone_out) { + auto drone = std::make_shared(); + if (!Ensure(drone->Initialize(client, world, name), "Initialize " + name)) return false; + *drone_out = drone; + return true; +} + +bool RunBasic(std::shared_ptr& client, std::shared_ptr& world) { + std::shared_ptr drone; + if (!InitDrone(client, world, "Drone1", &drone)) return false; + if (!EnableArmTakeoff(drone, "Drone1")) return false; + + auto move = drone->MoveByVelocityAsync(0.0f, 0.0f, -1.0f, 3.0f); + if (!Ensure(move.Wait(), "Drone1 MoveByVelocityAsync")) return false; + + return LandDisarm(drone, "Drone1"); +} + +bool TrySensorCall(std::shared_ptr& drone, + Status (pasc::Drone::*sensor_call)(const std::string&, json*) const, + const std::vector& sensor_names, + const std::string& label) { + for (const auto& sensor_name : sensor_names) { + json data; + Status status = ((*drone).*sensor_call)(sensor_name, &data); + if (status == Status::OK) { + std::cout << "[OK] " << label << " with sensor '" << sensor_name << "': " + << data.dump().substr(0, 220) << std::endl; + return true; + } + } + + std::cerr << "[WARN] No sensor worked for " << label << std::endl; + return false; +} + +bool RunSensors(std::shared_ptr& client, std::shared_ptr& world) { + std::shared_ptr drone; + if (!InitDrone(client, world, "Drone1", &drone)) return false; + if (!EnableArmTakeoff(drone, "Drone1")) return false; + + // Use standard sensor names from scene config + // (GetSensorCount/GetSensorName not yet implemented on server side, so use hardcoded list) + std::vector imu_sensors = {"IMU1"}; + std::vector gps_sensors = {"GPS"}; + std::vector baro_sensors = {"Barometer"}; + std::vector mag_sensors = {"Magnetometer"}; + std::vector airspeed_sensors = {"Airspeed"}; + std::vector camera_sensors = {"DownCamera", "Chase"}; + + std::cout << "[INFO] Testing standard sensor names from scene config" << std::endl; + + bool ok = true; + ok &= TrySensorCall(drone, &pasc::Drone::GetIMUData, imu_sensors, "GetIMUData"); + ok &= TrySensorCall(drone, &pasc::Drone::GetGPSData, gps_sensors, "GetGPSData"); + ok &= TrySensorCall(drone, &pasc::Drone::GetBarometerData, baro_sensors, + "GetBarometerData"); + ok &= TrySensorCall(drone, &pasc::Drone::GetMagnetometerData, mag_sensors, + "GetMagnetometerData"); + ok &= TrySensorCall(drone, &pasc::Drone::GetAirspeedData, airspeed_sensors, + "GetAirspeedData"); + + json image_data; + bool image_ok = false; + for (const auto& camera_name : camera_sensors) { + Status status = drone->GetImages(camera_name, {0}, &image_data); + if (status == Status::OK) { + std::cout << "[OK] GetImages with camera '" << camera_name << "'" << std::endl; + image_ok = true; + break; + } + } + + if (!image_ok) { + std::cerr << "[WARN] No camera sensor accepted GetImages" << std::endl; + } + + json battery_state; + if (Ensure(drone->GetBatteryState(&battery_state), "GetBatteryState")) { + std::cout << "[OK] Battery state: " << battery_state.dump().substr(0, 220) + << std::endl; + } else { + ok = false; + } + + if (!LandDisarm(drone, "Drone1")) return false; + return ok; +} + +bool RunTwoDrones(std::shared_ptr& client, std::shared_ptr& world) { + std::shared_ptr drone1; + std::shared_ptr drone2; + + if (!InitDrone(client, world, "Drone1", &drone1)) return false; + if (!InitDrone(client, world, "Drone2", &drone2)) return false; + + if (!EnableArmTakeoff(drone1, "Drone1")) return false; + if (!EnableArmTakeoff(drone2, "Drone2")) return false; + + // Move drones sequentially to avoid state conflicts + if (!Ensure(drone1->MoveByVelocityAsync(1.0f, 0.0f, 0.0f, 2.5f).Wait(), + "Drone1 MoveByVelocityAsync")) return false; + if (!Ensure(drone2->MoveByVelocityAsync(0.0f, 1.0f, 0.0f, 2.5f).Wait(), + "Drone2 MoveByVelocityAsync")) return false; + + json kinematics1; + json kinematics2; + Ensure(drone1->GetGroundTruthKinematics(&kinematics1), "Drone1 kinematics"); + Ensure(drone2->GetGroundTruthKinematics(&kinematics2), "Drone2 kinematics"); + + std::cout << "[OK] Drone1 kinematics sample: " + << kinematics1.dump().substr(0, 220) << std::endl; + std::cout << "[OK] Drone2 kinematics sample: " + << kinematics2.dump().substr(0, 220) << std::endl; + + if (!LandDisarm(drone1, "Drone1")) return false; + if (!LandDisarm(drone2, "Drone2")) return false; + + return true; +} + +bool RunWindAndWorld(std::shared_ptr& client, + std::shared_ptr& world) { + const std::string parent = world->GetParentTopic(); + + bool ok = true; + json response; + + ok &= RequestJson(client, parent + "/SetSunLightIntensity", {{"intensity", 12000.0f}}); + ok &= RequestJson(client, parent + "/GetSunLightIntensity", {}, &response); + std::cout << "[OK] Sun intensity: " << response.dump() << std::endl; + + ok &= RequestJson(client, parent + "/SetCloudShadowStrength", {{"strength", 0.35f}}); + ok &= RequestJson(client, parent + "/GetCloudShadowStrength", {}, &response); + std::cout << "[OK] Cloud shadow strength: " << response.dump() << std::endl; + + ok &= RequestJson(client, parent + "/SetWindVelocity", + {{"v_x", 2.0f}, {"v_y", 1.0f}, {"v_z", 0.0f}}); + ok &= RequestJson(client, parent + "/GetWindVelocity", {}, &response); + std::cout << "[OK] Wind velocity: " << response.dump() << std::endl; + + ok &= RequestJson(client, parent + "/Pause", {{"do_pause", true}}); + ok &= RequestJson(client, parent + "/GetSimTime", {}, &response); + std::cout << "[OK] Sim time (paused): " << response.dump() << std::endl; + ok &= RequestJson(client, parent + "/Pause", {{"do_pause", false}}); + + return ok; +} + +bool RunBattery(std::shared_ptr& client, std::shared_ptr& world) { + std::shared_ptr drone; + if (!InitDrone(client, world, "Drone1", &drone)) return false; + + json battery; + if (!Ensure(drone->GetBatteryState(&battery), "GetBatteryState")) return false; + std::cout << "[OK] Initial battery: " << battery.dump().substr(0, 220) << std::endl; + + bool set_ok = false; + if (!Ensure(drone->SetBatteryDrainRate(0.05f, &set_ok), "SetBatteryDrainRate")) + return false; + + float drain_rate = 0.0f; + if (!Ensure(drone->GetBatteryDrainRate(&drain_rate), "GetBatteryDrainRate")) + return false; + + std::cout << "[OK] Drain rate now: " << drain_rate << std::endl; + + if (!EnableArmTakeoff(drone, "Drone1")) return false; + auto hover = drone->HoverAsync(); + if (!Ensure(hover.Wait(), "HoverAsync")) return false; + + return LandDisarm(drone, "Drone1"); +} + +bool RunRover(std::shared_ptr& client, std::shared_ptr& world) { + std::vector topics = client->GetTopicInfo(); + if (topics.empty()) { + std::cerr << "[ERROR] No topics were returned by the server." << std::endl; + return false; + } + + size_t rover_topic_count = 0; + for (const auto& topic : topics) { + if (topic.find("/Rover1/") != std::string::npos) { + ++rover_topic_count; + } + } + + if (rover_topic_count == 0) { + std::cerr << "[ERROR] Rover topics were not discovered for Rover1." << std::endl; + return false; + } + + std::cout << "[OK] Rover topics discovered for Rover1: " << rover_topic_count + << std::endl; + + // Exercise world-level services on the rover scene even though Rover API is not + // implemented yet in the C++ client library. + return RunWindAndWorld(client, world); +} + +int main(int argc, const char* argv[]) { + std::string scenario = "basic"; + std::string sim_host = "127.0.0.1"; + std::string sim_config = "client/python/example_user_scripts/sim_config"; + + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--scenario") { + if (++i >= argc) { + std::cerr << "Missing value for --scenario" << std::endl; + return 1; + } + scenario = argv[i]; + } else if (arg == "--simhost") { + if (++i >= argc) { + std::cerr << "Missing value for --simhost" << std::endl; + return 1; + } + sim_host = argv[i]; + } else if (arg == "--simconfig") { + if (++i >= argc) { + std::cerr << "Missing value for --simconfig" << std::endl; + return 1; + } + sim_config = argv[i]; + } else if (arg == "-h" || arg == "--help") { + std::cout << "cpp_client_scenarios --scenario " + "[--simhost 127.0.0.1] [--simconfig path]" + << std::endl; + return 0; + } else { + std::cerr << "Unknown argument: " << arg << std::endl; + return 1; + } + } + + std::string scene_file; + if (scenario == "basic") { + scene_file = "scene_basic_drone.jsonc"; + } else if (scenario == "sensors") { + scene_file = "scene_drone_sensors.jsonc"; + } else if (scenario == "two_drones") { + scene_file = "scene_two_drones.jsonc"; + } else if (scenario == "wind") { + scene_file = "scene_drone_wind.jsonc"; + } else if (scenario == "battery") { + scene_file = "scene_battery_simple.jsonc"; + } else if (scenario == "rover") { + scene_file = "scene_basic_rover.jsonc"; + } else { + std::cerr << "Unknown scenario: " << scenario << std::endl; + return 1; + } + + pasc::log.SetLogSink(LogSink); + + std::filesystem::path sim_path; + std::filesystem::path scene_path; + try { + sim_path = std::filesystem::canonical(sim_config); + scene_path = sim_path / scene_file; + } catch (const std::filesystem::filesystem_error& ex) { + std::cerr << "simconfig path error: " << ex.what() << std::endl; + return 1; + } + + auto client = std::make_shared(); + if (!Ensure(client->Connect(sim_host), "Client Connect")) return 1; + + auto world = std::make_shared(); + if (!Ensure(world->Initialize(client, scene_path.string(), sim_path.string(), 2.0f), + "World Initialize")) { + return 1; + } + + bool ok = false; + if (scenario == "basic") { + ok = RunBasic(client, world); + } else if (scenario == "sensors") { + ok = RunSensors(client, world); + } else if (scenario == "two_drones") { + ok = RunTwoDrones(client, world); + } else if (scenario == "wind") { + ok = RunWindAndWorld(client, world); + } else if (scenario == "battery") { + ok = RunBattery(client, world); + } else if (scenario == "rover") { + ok = RunRover(client, world); + } + + client->Disconnect(); + + if (!ok) { + std::cerr << "[FAIL] Scenario '" << scenario << "' failed" << std::endl; + return 1; + } + + std::cout << "[PASS] Scenario '" << scenario << "' completed" << std::endl; + return 0; +} diff --git a/client/cpp/example_user_apps/HelloDrone/HelloDrone.cpp b/client/cpp/example_user_apps/HelloDrone/HelloDrone.cpp index 72a0defe..8db03855 100644 --- a/client/cpp/example_user_apps/HelloDrone/HelloDrone.cpp +++ b/client/cpp/example_user_apps/HelloDrone/HelloDrone.cpp @@ -6,26 +6,45 @@ // Demonstrates flying a quadrotor drone. // -#include +#include +#include #include +#include #include -namespace mpc = microsoft::projectairsim::client; -using mpc::Status; +namespace pasc = microsoft::projectairsim::client; +using pasc::Status; -void MyLogOutputSink(mpc::Log::Severity /*severity*/, +void MyLogOutputSink(pasc::Log::Severity /*severity*/, const char* sz_message) noexcept { // Route log output to standard output std::cout << sz_message << std::endl; } +bool IsCaseInsensitiveEqual(const char* lhs, const char* rhs) { + std::string_view left(lhs == nullptr ? "" : lhs); + std::string_view right(rhs == nullptr ? "" : rhs); + + if (left.size() != right.size()) return false; + + for (size_t i = 0; i < left.size(); ++i) { + if (std::tolower(static_cast(left[i])) != + std::tolower(static_cast(right[i]))) + return false; + } + + return true; +} + int main(int argc, const char* argv[]) { - mpc::AsyncResult asyncresult; + pasc::AsyncResult asyncresult; bool f; - std::shared_ptr pclient; - std::shared_ptr pdrone; - std::shared_ptr pworld; + pasc::LandedState landed_state; + pasc::ReadyState ready_state; + std::shared_ptr pclient; + std::shared_ptr pdrone; + std::shared_ptr pworld; Status status; std::string strSimulationHost = "localhost"; std::string strSceneFile = "scene_basic_drone.jsonc"; @@ -33,8 +52,9 @@ int main(int argc, const char* argv[]) { // Parse arguments if (argc > 1) { - if ((_stricmp(argv[1], "/?") == 0) || (_stricmp(argv[1], "-h") == 0) || - (_stricmp(argv[1], "--help") == 0)) { + if (IsCaseInsensitiveEqual(argv[1], "/?") || + IsCaseInsensitiveEqual(argv[1], "-h") || + IsCaseInsensitiveEqual(argv[1], "--help")) { std::cout << "Perform a simple drone takeoff, move up, and landing " "in Project AirSim." << std::endl @@ -72,7 +92,7 @@ int main(int argc, const char* argv[]) { std::filesystem::canonical(*pszArg); strPathSimConfig = pathConfig.string(); strSceneFile = (pathConfig / strSceneFile).string(); - } catch (std::filesystem::filesystem_error e) { + } catch (const std::filesystem::filesystem_error& e) { std::cerr << "sim_config path error: " << e.what() << std::endl; return (1); } @@ -85,21 +105,21 @@ int main(int argc, const char* argv[]) { } // Set custom log output handler - mpc::log.SetLogSink(MyLogOutputSink); + pasc::log.SetLogSink(MyLogOutputSink); // Connect to simulation environment - pclient.reset(new mpc::Client()); + pclient.reset(new pasc::Client()); if ((status = pclient->Connect(strSimulationHost)) != Status::OK) goto LError; // Create a World object to interact with the sim world and load a scene - pworld.reset(new mpc::World()); - mpc::log.InfoF("Looking for scene file \"%s\"", strSceneFile.c_str()); + pworld.reset(new pasc::World()); + pasc::log.InfoF("Looking for scene file \"%s\"", strSceneFile.c_str()); if ((status = pworld->Initialize(pclient, strSceneFile, strPathSimConfig, 2.0f)) != Status::OK) goto LError; // Connect to the drone - pdrone.reset(new mpc::Drone()); + pdrone.reset(new pasc::Drone()); if ((status = pdrone->Initialize(pclient, pworld, "Drone1")) != Status::OK) goto LError; @@ -110,16 +130,20 @@ int main(int argc, const char* argv[]) { ((status = pdrone->Arm(&f)) != Status::OK)) { goto LError; } + if ((status = pdrone->GetReadyState(&ready_state)) != Status::OK) goto LError; + pasc::log.InfoF("Ready state: %s (%s)", + ready_state.is_ready ? "ready" : "not ready", + ready_state.message.c_str()); /*----------------------------------------------------------------------*/ // Launch the drone - mpc::log.Info("TakeoffAsync: starting"); + pasc::log.Info("TakeoffAsync: starting"); asyncresult = pdrone->TakeoffAsync(); // Example 1: Wait on the result of async operation if ((status = asyncresult.Wait()) != Status::OK) goto LError; - mpc::log.Info("TakeoffAsync: completed"); + pasc::log.Info("TakeoffAsync: completed"); /*----------------------------------------------------------------------*/ @@ -129,7 +153,7 @@ int main(int argc, const char* argv[]) { 0.0f, // v_east -1.0f, // v_down 4.0); // sec_duration - mpc::log.Info("Move-Up invoked"); + pasc::log.Info("Move-Up invoked"); // Example 2: Wait for async task to complete before continuing while (!asyncresult.FIsDone()) @@ -137,15 +161,20 @@ int main(int argc, const char* argv[]) { // Note: must call AsyncResult::Wait() even though AsyncResult::FIsDone() was used if ((status = asyncresult.Wait()) != Status::OK) goto LError; - mpc::log.Info("Move-Up completed"); + pasc::log.Info("Move-Up completed"); /*----------------------------------------------------------------------*/ // Land the drone - mpc::log.Info("LandAsync: starting"); + pasc::log.Info("LandAsync: starting"); asyncresult = pdrone->LandAsync(); if ((status = asyncresult.Wait()) != Status::OK) goto LError; - mpc::log.Info("LandAsync: completed"); + if ((status = pdrone->GetLandedState(&landed_state)) != Status::OK) + goto LError; + pasc::log.InfoF("Landed state: %s", + landed_state == pasc::LandedState::Landed ? "landed" + : "flying"); + pasc::log.Info("LandAsync: completed"); /*----------------------------------------------------------------------*/ @@ -158,7 +187,7 @@ int main(int argc, const char* argv[]) { /*----------------------------------------------------------------------*/ // All done! - mpc::log.Info("Drone landed."); + pasc::log.Info("Drone landed."); pclient->Disconnect(); return (0); @@ -166,8 +195,8 @@ int main(int argc, const char* argv[]) { LError : { char szErr[256]; - mpc::GetStatusString(status, szErr); - mpc::log.Critical(szErr); + pasc::GetStatusString(status, szErr); + pasc::log.Critical(szErr); } return (1); diff --git a/client/cpp/example_user_apps/HelloDrone/HelloDrone.vcxproj b/client/cpp/example_user_apps/HelloDrone/HelloDrone.vcxproj index 770d6d8a..d89df3dd 100644 --- a/client/cpp/example_user_apps/HelloDrone/HelloDrone.vcxproj +++ b/client/cpp/example_user_apps/HelloDrone/HelloDrone.vcxproj @@ -30,26 +30,26 @@ Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode @@ -101,13 +101,13 @@ true WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(ProjectDir)..\..\AirSimClientDLL\include\;$(ProjectDir)..\..\AirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) + $(ProjectDir)..\..\ProjectAirsimClientLib\include\;$(ProjectDir)..\..\ProjectAirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) stdcpp17 Console true - AirSimClient.lib;NNGI.lib;%(AdditionalDependencies) + ProjectAirsimClientLib.2022.lib;NNGI.2022.lib;%(AdditionalDependencies) @@ -118,7 +118,7 @@ true WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(ProjectDir)..\..\AirSimClientDLL\include\;$(ProjectDir)..\..\AirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) + $(ProjectDir)..\..\ProjectAirsimClientLib\include\;$(ProjectDir)..\..\ProjectAirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) stdcpp17 @@ -126,7 +126,7 @@ true true true - AirSimClient.lib;NNGI.lib;%(AdditionalDependencies) + ProjectAirsimClientLib.2022.lib;NNGI.2022.lib;%(AdditionalDependencies) @@ -135,13 +135,13 @@ true _DEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(ProjectDir)..\..\AirSimClientDLL\include\;$(ProjectDir)..\..\AirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) + $(ProjectDir)..\..\ProjectAirsimClientLib\include\;$(ProjectDir)..\..\ProjectAirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) stdcpp17 Console true - AirSimClient.lib;NNGI.lib;%(AdditionalDependencies) + ProjectAirsimClientLib.2022.lib;NNGI.2022.lib;%(AdditionalDependencies) @@ -152,7 +152,7 @@ true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(ProjectDir)..\..\AirSimClientDLL\include\;$(ProjectDir)..\..\AirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) + $(ProjectDir)..\..\ProjectAirsimClientLib\include\;$(ProjectDir)..\..\ProjectAirSimMessageLib\include\;$(ProjectDir)..\..\..\..\core_sim\include\;$(ProjectDir)..\..\..\..\build\win$(PlatformArchitecture)\$(Configuration)\_deps\nlohmann-install;%(AdditionalIncludeDirectories) stdcpp17 @@ -160,7 +160,7 @@ true true true - AirSimClient.lib;NNGI.lib;%(AdditionalDependencies) + ProjectAirsimClientLib.2022.lib;NNGI.2022.lib;%(AdditionalDependencies) @@ -172,4 +172,4 @@ - \ No newline at end of file + diff --git a/client/cpp/example_user_apps/HelloDrone/README.md b/client/cpp/example_user_apps/HelloDrone/README.md index 6a15473e..7ffa3e64 100644 --- a/client/cpp/example_user_apps/HelloDrone/README.md +++ b/client/cpp/example_user_apps/HelloDrone/README.md @@ -5,15 +5,38 @@ Project AirSim simulation through the Project AirSim C++ client library. ## Building: + +### Windows (Visual Studio) 1. Open the solution file "HelloDrone.sln" in Visual Studio 2019. 2. Build the project (Build-->"Build Solution".) 3. The executable `hello_drone.exe` is in the project output directory. -4. Note that `hello_drone.exe` imports the `AirSimClient.dll` and `NNGI.dll` +4. Note that `hello_drone.exe` imports the `ProjectAirsimClient.dll` and `NNGI.dll` dynamically-loaded libraries. These two files are in the client\cpp\libraries\\build_platform\\build_configuration\\ directory. Add this directory to the `PATH` environment variable or copy those DLL's to the directory containing `hello_drone.exe`. +### Linux (CMake) +From the repository root: + +```bash +cd client/cpp +cmake -S . -B build_linux -DCMAKE_BUILD_TYPE=Debug +cmake --build build_linux -j$(nproc) +``` + +The executable is generated at: + +```text +client/cpp/build_linux/hello_drone +``` + +Run help to verify: + +```bash +./client/cpp/build_linux/hello_drone --help +``` + ## Running: 1. Before running the Hello Drone application, launch a basic Project AirSim simulation environment. diff --git a/client/cpp/example_user_apps/UserEnvActorScenario/UserEnvActorScenario.cpp b/client/cpp/example_user_apps/UserEnvActorScenario/UserEnvActorScenario.cpp new file mode 100644 index 00000000..3da158ff --- /dev/null +++ b/client/cpp/example_user_apps/UserEnvActorScenario/UserEnvActorScenario.cpp @@ -0,0 +1,224 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pasc = microsoft::projectairsim::client; +using pasc::Status; + +namespace { + +void LogSink(pasc::Log::Severity /*severity*/, const char* message) noexcept { + std::cout << message << std::endl; +} + +std::string StatusToString(Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +bool Ensure(Status status, const std::string& where) { + if (status == Status::OK) return true; + std::cerr << "[ERROR] " << where << ": " << StatusToString(status) + << std::endl; + return false; +} + +void GetLoopTheLoopCoords(float radius, float center_y, float center_z, + int num_points, std::vector* y_coords, + std::vector* z_coords) { + constexpr float kPi = 3.14159265358979323846f; + const float angle_step = 2.0f * kPi / static_cast(num_points); + + std::vector angles; + angles.reserve(num_points + num_points / 2 + 1); + for (int idx = 0; idx <= num_points; ++idx) { + angles.emplace_back(-kPi + angle_step * static_cast(idx)); + } + + const float angle_start = angles.back(); + for (int idx = 1; idx <= num_points / 2; ++idx) { + angles.emplace_back(angle_start + angle_step * static_cast(idx)); + } + + y_coords->clear(); + z_coords->clear(); + y_coords->reserve(angles.size()); + z_coords->reserve(angles.size()); + for (float angle : angles) { + y_coords->emplace_back(center_y + radius * std::cos(angle)); + z_coords->emplace_back(center_z + radius * std::sin(angle)); + } +} + +bool RunEnvActorScenario(std::shared_ptr& client, + std::shared_ptr& world) { + pasc::EnvActor actor_with_config_traj; + pasc::EnvActor tiltrotor_with_config_traj_offset; + pasc::EnvActor actor_with_api_traj; + pasc::EnvActor actor_with_api_traj_and_offset; + + if (!Ensure(actor_with_config_traj.Initialize(client, world, + "ActorWithConfigTraj"), + "Initialize ActorWithConfigTraj")) + return false; + if (!Ensure(tiltrotor_with_config_traj_offset.Initialize( + client, world, "TiltrotorWithConfigTrajOffset"), + "Initialize TiltrotorWithConfigTrajOffset")) + return false; + if (!Ensure(actor_with_api_traj.Initialize(client, world, "ActorWithApiTraj"), + "Initialize ActorWithApiTraj")) + return false; + if (!Ensure(actor_with_api_traj_and_offset.Initialize( + client, world, "ActorWithApiTrajAndOffset"), + "Initialize ActorWithApiTrajAndOffset")) + return false; + + std::cout << "[..] Setting TiltrotorWithConfigTrajOffset trajectory to " + "right_and_descend_config" + << std::endl; + const std::map shroud_angle_map = { + {"Shroud_FL", 90.0f}, + {"Shroud_RL", 90.0f}, + {"Shroud_FR", 90.0f}, + {"Shroud_RR", 90.0f}, + }; + if (!Ensure(tiltrotor_with_config_traj_offset.SetLinkRotationAngles( + shroud_angle_map), + "SetLinkRotationAngles")) + return false; + + if (!Ensure(tiltrotor_with_config_traj_offset.SetTrajectory( + "right_and_descend_config", true, 3.0f, 2.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.57f), + "SetTrajectory right_and_descend_config")) + return false; + + std::vector time_sec = {4.0f}; + std::vector pose_x = {1.0f}; + std::vector pose_y = {0.0f}; + std::vector pose_z = {-6.0f}; + + std::vector y_looptheloop; + std::vector z_looptheloop; + GetLoopTheLoopCoords(3.0f, 10.0f, -6.0f, 20, &y_looptheloop, + &z_looptheloop); + + const float step_size = 0.2f; + const float looptheloop_start = time_sec.back() + 2.0f; + for (size_t idx = 0; idx < y_looptheloop.size(); ++idx) { + time_sec.emplace_back(looptheloop_start + + step_size * static_cast(idx + 1)); + pose_x.emplace_back(pose_x.back()); + pose_y.emplace_back(y_looptheloop[idx]); + pose_z.emplace_back(z_looptheloop[idx]); + } + + time_sec.emplace_back(time_sec.back() + 2.0f); + pose_x.emplace_back(pose_x.back()); + pose_y.emplace_back(pose_y.back() + 3.0f); + pose_z.emplace_back(pose_z.back()); + + std::cout << "[..] Importing looptheloop NED trajectory" << std::endl; + if (!Ensure(world->ImportNEDTrajectory("looptheloop", time_sec, pose_x, + pose_y, pose_z), + "ImportNEDTrajectory looptheloop")) + return false; + + std::cout << "[..] Setting looptheloop trajectory to API actors" + << std::endl; + if (!Ensure(actor_with_api_traj.SetTrajectory("looptheloop", true), + "SetTrajectory ActorWithApiTraj")) + return false; + if (!Ensure(actor_with_api_traj_and_offset.SetTrajectory("looptheloop", true, + 2.0f), + "SetTrajectory ActorWithApiTrajAndOffset")) + return false; + + std::cout << "[OK] EnvActor trajectories configured" << std::endl; + return true; +} + +} // namespace + +int main(int argc, const char* argv[]) { + std::string sim_host = "127.0.0.1"; + std::string sim_config = "client/python/example_user_scripts/sim_config"; + std::string scene_file = "scene_env_actor.jsonc"; + + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (arg == "--simhost") { + if (++i >= argc) { + std::cerr << "Missing value for --simhost" << std::endl; + return 1; + } + sim_host = argv[i]; + } else if (arg == "--simconfig") { + if (++i >= argc) { + std::cerr << "Missing value for --simconfig" << std::endl; + return 1; + } + sim_config = argv[i]; + } else if (arg == "--scene") { + if (++i >= argc) { + std::cerr << "Missing value for --scene" << std::endl; + return 1; + } + scene_file = argv[i]; + } else if (arg == "-h" || arg == "--help") { + std::cout + << "user_env_actor_scenario [--simhost 127.0.0.1] [--simconfig path] " + "[--scene scene_env_actor.jsonc]" + << std::endl; + return 0; + } else { + std::cerr << "Unknown argument: " << arg << std::endl; + return 1; + } + } + + pasc::log.SetLogSink(LogSink); + + std::filesystem::path sim_path; + std::filesystem::path scene_path; + try { + sim_path = std::filesystem::canonical(sim_config); + scene_path = sim_path / scene_file; + } catch (const std::filesystem::filesystem_error& ex) { + std::cerr << "simconfig path error: " << ex.what() << std::endl; + return 1; + } + + auto client = std::make_shared(); + if (!Ensure(client->Connect(sim_host), "Client Connect")) return 1; + + auto world = std::make_shared(); + if (!Ensure(world->Initialize(client, scene_path.string(), sim_path.string(), + 2.0f), + "World Initialize")) { + client->Disconnect(); + return 1; + } + + const bool ok = RunEnvActorScenario(client, world); + client->Disconnect(); + + if (!ok) { + std::cerr << "[FAIL] EnvActor scenario failed" << std::endl; + return 1; + } + + std::cout << "[PASS] EnvActor scenario completed" << std::endl; + return 0; +} diff --git a/client/cpp/example_user_apps/UserLidarScenario/UserLidarScenario.cpp b/client/cpp/example_user_apps/UserLidarScenario/UserLidarScenario.cpp new file mode 100644 index 00000000..92de04ca --- /dev/null +++ b/client/cpp/example_user_apps/UserLidarScenario/UserLidarScenario.cpp @@ -0,0 +1,242 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include +#include +#include + +#include "msgpack.hpp" + +namespace pasc = microsoft::projectairsim::client; +using pasc::Status; +using nlohmann::json; + +namespace { + +void LogSink(pasc::Log::Severity /*severity*/, const char* message) noexcept { + std::cout << message << std::endl; +} + +std::string StatusToString(Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +bool Ensure(Status status, const std::string& where) { + if (status == Status::OK) return true; + std::cerr << "[ERROR] " << where << ": " << StatusToString(status) + << std::endl; + return false; +} + +size_t CountLidarPoints(const std::string& msg) { + auto handle = msgpack::unpack(msg.data(), msg.size()); + msgpack::object object = handle.get(); + if (object.type != msgpack::type::MAP) return 0; + + for (uint32_t i = 0; i < object.via.map.size; ++i) { + const auto& key = object.via.map.ptr[i].key; + const auto& value = object.via.map.ptr[i].val; + if (key.type != msgpack::type::STR) continue; + + const std::string key_string(key.via.str.ptr, key.via.str.size); + if (key_string == "point_cloud" && value.type == msgpack::type::ARRAY) { + return value.via.array.size / 3; + } + } + + return 0; +} + +bool RunLidarScenario(std::shared_ptr& client, + std::shared_ptr& world, + const std::string& vehicle_name, + const std::string& lidar_name) { + // -- Initialize drone -- + pasc::Drone drone; + if (!Ensure(drone.Initialize(client, world, vehicle_name), + "Drone Initialize")) + return false; + std::cout << "[OK] Drone '" << vehicle_name << "' initialized" << std::endl; + + // -- Enable API control -- + bool api_enabled = false; + if (!Ensure(drone.EnableAPIControl(&api_enabled), "EnableAPIControl")) + return false; + std::cout << "[OK] API control enabled" << std::endl; + + // -- Arm drone -- + bool armed = false; + if (!Ensure(drone.Arm(&armed), "Arm")) return false; + std::cout << "[OK] Drone armed" << std::endl; + + // -- Take off -- + std::cout << "[..] Takeoff..." << std::endl; + auto async_result = drone.TakeoffAsync(/*sec_timeout*/ 10.0f); + if (!Ensure(async_result.Wait(), "Takeoff")) return false; + std::cout << "[OK] Drone at altitude" << std::endl; + + // -- Retrieve Lidar data from the published topic -- + const char* lidar_topic = drone.GetSensorTopic(lidar_name, "lidar"); + if (lidar_topic == nullptr || std::string(lidar_topic).empty()) { + std::cerr << "[ERROR] Lidar topic not found for sensor '" << lidar_name + << "'" << std::endl; + return false; + } + + std::cout << "[INFO] Subscribing to Lidar topic: " << lidar_topic + << std::endl; + + std::mutex mutex; + std::condition_variable cv; + int frame_count = 0; + bool parse_error = false; + + if (!Ensure(client->Subscribe( + lidar_topic, + [&](const std::string& /*topic*/, const std::string& msg) { + try { + const size_t point_count = CountLidarPoints(msg); + + { + std::lock_guard lock(mutex); + ++frame_count; + } + std::cout << "[OK] Lidar frame " << frame_count + << " received with " << point_count + << " points" << std::endl; + cv.notify_one(); + } catch (const std::exception& ex) { + std::lock_guard lock(mutex); + parse_error = true; + std::cerr << "[ERROR] Failed to parse Lidar message: " + << ex.what() << std::endl; + cv.notify_one(); + } + }), + "Subscribe Lidar topic")) + return false; + + { + std::unique_lock lock(mutex); + const bool got_frames = cv.wait_for(lock, std::chrono::seconds(5), [&] { + return frame_count >= 3 || parse_error; + }); + if (!got_frames) { + std::cerr << "[ERROR] Timed out waiting for Lidar topic messages" + << std::endl; + client->Unsubscribe(lidar_topic); + return false; + } + if (parse_error) { + client->Unsubscribe(lidar_topic); + return false; + } + } + client->Unsubscribe(lidar_topic); + + // -- Land drone -- + std::cout << "[..] Landing..." << std::endl; + auto land_result = drone.LandAsync(/*sec_timeout*/ 10.0f); + if (!Ensure(land_result.Wait(), "Land")) return false; + std::cout << "[OK] Drone landed" << std::endl; + + return true; +} + +} // namespace + +int main(int argc, char* argv[]) { + using std::filesystem::exists; + + pasc::log.SetLogSink(LogSink); + + try { + // -- Configuration -- + std::string sim_host = "127.0.0.1"; + std::string sim_config = "client/python/example_user_scripts/sim_config"; + std::string scene_file = "scene_lidar_drone.jsonc"; + std::string vehicle_name = "Drone1"; + std::string lidar_name = "lidar1"; + + // Parse command line arguments + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--simhost") { + if (++i >= argc) { + std::cerr << "Missing value for --simhost" << std::endl; + return 1; + } + sim_host = argv[i]; + } else if (arg == "--vehicle" && i + 1 < argc) { + vehicle_name = argv[++i]; + } else if (arg == "--lidar" && i + 1 < argc) { + lidar_name = argv[++i]; + } else if (arg == "--simconfig" && i + 1 < argc) { + sim_config = argv[++i]; + } else if (arg == "--scene" && i + 1 < argc) { + scene_file = argv[++i]; + } else if (arg == "-h" || arg == "--help") { + std::cout << "user_lidar_scenario [--simhost 127.0.0.1] [--simconfig path] " + "[--scene scene_lidar_drone.jsonc] [--vehicle Drone1] " + "[--lidar Lidar1]" + << std::endl; + return 0; + } else { + std::cerr << "Unknown argument: " << arg << std::endl; + return 1; + } + } + + std::filesystem::path sim_path; + std::filesystem::path scene_path; + try { + sim_path = std::filesystem::canonical(sim_config); + scene_path = sim_path / scene_file; + } catch (const std::filesystem::filesystem_error& ex) { + std::cerr << "simconfig path error: " << ex.what() << std::endl; + return 1; + } + + // -- Create client and world -- + auto client = std::make_shared(); + std::cout << "[INFO] Connecting to AirSim at " << sim_host << std::endl; + if (!Ensure(client->Connect(sim_host), "Client Connect")) return 1; + std::cout << "[OK] Client connected" << std::endl; + + auto world = std::make_shared(); + if (!Ensure(world->Initialize(client, scene_path.string(), sim_path.string(), + 2.0f), + "World Initialize")) { + client->Disconnect(); + return 1; + } + std::cout << "[OK] World initialized" << std::endl; + + // -- Run scenario -- + std::cout << "[INFO] Running Lidar scenario for vehicle: " << vehicle_name + << ", sensor: " << lidar_name << std::endl; + if (!RunLidarScenario(client, world, vehicle_name, lidar_name)) { + client->Disconnect(); + return 1; + } + + client->Disconnect(); + std::cout << "[OK] Lidar scenario completed successfully" << std::endl; + return 0; + + } catch (const std::exception& ex) { + std::cerr << "[ERROR] Exception: " << ex.what() << std::endl; + return 1; + } catch (...) { + std::cerr << "[ERROR] Unknown exception" << std::endl; + return 1; + } +} diff --git a/client/cpp/example_user_apps/UserRadarScenario/UserRadarScenario.cpp b/client/cpp/example_user_apps/UserRadarScenario/UserRadarScenario.cpp new file mode 100644 index 00000000..a7a55103 --- /dev/null +++ b/client/cpp/example_user_apps/UserRadarScenario/UserRadarScenario.cpp @@ -0,0 +1,257 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include +#include +#include + +#include "msgpack.hpp" + +namespace pasc = microsoft::projectairsim::client; +using pasc::Status; +using nlohmann::json; + +namespace { + +void LogSink(pasc::Log::Severity /*severity*/, const char* message) noexcept { + std::cout << message << std::endl; +} + +std::string StatusToString(Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +bool Ensure(Status status, const std::string& where) { + if (status == Status::OK) return true; + std::cerr << "[ERROR] " << where << ": " << StatusToString(status) + << std::endl; + return false; +} + +size_t CountMsgpackArrayField(const std::string& msg, + const std::string& field_name) { + auto handle = msgpack::unpack(msg.data(), msg.size()); + msgpack::object object = handle.get(); + if (object.type != msgpack::type::MAP) return 0; + + for (uint32_t i = 0; i < object.via.map.size; ++i) { + const auto& key = object.via.map.ptr[i].key; + const auto& value = object.via.map.ptr[i].val; + if (key.type != msgpack::type::STR) continue; + + const std::string key_string(key.via.str.ptr, key.via.str.size); + if (key_string == field_name && value.type == msgpack::type::ARRAY) { + return value.via.array.size; + } + } + + return 0; +} + +bool RunRadarScenario(std::shared_ptr& client, + std::shared_ptr& world, + const std::string& vehicle_name, + const std::string& radar_name) { + // -- Initialize drone -- + pasc::Drone drone; + if (!Ensure(drone.Initialize(client, world, vehicle_name), + "Drone Initialize")) + return false; + std::cout << "[OK] Drone '" << vehicle_name << "' initialized" << std::endl; + + const char* detections_topic = + drone.GetSensorTopic(radar_name, "radar_detections"); + const char* tracks_topic = drone.GetSensorTopic(radar_name, "radar_tracks"); + if (detections_topic == nullptr || std::string(detections_topic).empty() || + tracks_topic == nullptr || std::string(tracks_topic).empty()) { + std::cerr << "[ERROR] Radar topics not found for sensor '" << radar_name + << "'" << std::endl; + return false; + } + + std::cout << "[INFO] Subscribing to Radar detections topic: " + << detections_topic << std::endl; + std::cout << "[INFO] Subscribing to Radar tracks topic: " << tracks_topic + << std::endl; + + std::mutex mutex; + std::condition_variable cv; + int detection_frames = 0; + int track_frames = 0; + bool parse_error = false; + + if (!Ensure(client->Subscribe( + detections_topic, + [&](const std::string& /*topic*/, const std::string& msg) { + try { + const size_t detection_count = + CountMsgpackArrayField(msg, "radar_detections"); + { + std::lock_guard lock(mutex); + ++detection_frames; + } + std::cout << "[OK] Radar detections frame " + << detection_frames << " received with " + << detection_count << " detections" + << std::endl; + cv.notify_one(); + } catch (const std::exception& ex) { + std::lock_guard lock(mutex); + parse_error = true; + std::cerr << "[ERROR] Failed to parse Radar detections: " + << ex.what() << std::endl; + cv.notify_one(); + } + }), + "Subscribe Radar detections")) + return false; + + if (!Ensure(client->Subscribe( + tracks_topic, + [&](const std::string& /*topic*/, const std::string& msg) { + try { + const size_t track_count = + CountMsgpackArrayField(msg, "radar_tracks"); + { + std::lock_guard lock(mutex); + ++track_frames; + } + std::cout << "[OK] Radar tracks frame " << track_frames + << " received with " << track_count + << " tracks" << std::endl; + cv.notify_one(); + } catch (const std::exception& ex) { + std::lock_guard lock(mutex); + parse_error = true; + std::cerr << "[ERROR] Failed to parse Radar tracks: " + << ex.what() << std::endl; + cv.notify_one(); + } + }), + "Subscribe Radar tracks")) { + client->Unsubscribe(detections_topic); + return false; + } + + { + std::unique_lock lock(mutex); + const bool got_frames = cv.wait_for(lock, std::chrono::seconds(10), [&] { + return (detection_frames >= 3 && track_frames >= 3) || parse_error; + }); + if (!got_frames) { + std::cerr << "[ERROR] Timed out waiting for Radar topic messages" + << std::endl; + client->Unsubscribe(detections_topic); + client->Unsubscribe(tracks_topic); + return false; + } + if (parse_error) { + client->Unsubscribe(detections_topic); + client->Unsubscribe(tracks_topic); + return false; + } + } + + client->Unsubscribe(detections_topic); + client->Unsubscribe(tracks_topic); + + return true; +} + +} // namespace + +int main(int argc, char* argv[]) { + using std::filesystem::exists; + + pasc::log.SetLogSink(LogSink); + + try { + // -- Configuration -- + std::string sim_host = "127.0.0.1"; + std::string sim_config = "client/python/example_user_scripts/sim_config"; + std::string scene_file = "scene_radar_tower.jsonc"; + std::string vehicle_name = "RadarTower1"; + std::string radar_name = "radar1"; + + // Parse command line arguments + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--simhost") { + if (++i >= argc) { + std::cerr << "Missing value for --simhost" << std::endl; + return 1; + } + sim_host = argv[i]; + } else if (arg == "--vehicle" && i + 1 < argc) { + vehicle_name = argv[++i]; + } else if (arg == "--radar" && i + 1 < argc) { + radar_name = argv[++i]; + } else if (arg == "--simconfig" && i + 1 < argc) { + sim_config = argv[++i]; + } else if (arg == "--scene" && i + 1 < argc) { + scene_file = argv[++i]; + } else if (arg == "-h" || arg == "--help") { + std::cout << "user_radar_scenario [--simhost 127.0.0.1] [--simconfig path] " + "[--scene scene_radar_tower.jsonc] [--vehicle RadarTower1] " + "[--radar radar1]" + << std::endl; + return 0; + } else { + std::cerr << "Unknown argument: " << arg << std::endl; + return 1; + } + } + + std::filesystem::path sim_path; + std::filesystem::path scene_path; + try { + sim_path = std::filesystem::canonical(sim_config); + scene_path = sim_path / scene_file; + } catch (const std::filesystem::filesystem_error& ex) { + std::cerr << "simconfig path error: " << ex.what() << std::endl; + return 1; + } + + // -- Create client and world -- + auto client = std::make_shared(); + std::cout << "[INFO] Connecting to AirSim at " << sim_host << std::endl; + if (!Ensure(client->Connect(sim_host), "Client Connect")) return 1; + std::cout << "[OK] Client connected" << std::endl; + + auto world = std::make_shared(); + if (!Ensure(world->Initialize(client, scene_path.string(), sim_path.string(), + 2.0f), + "World Initialize")) { + client->Disconnect(); + return 1; + } + std::cout << "[OK] World initialized" << std::endl; + + // -- Run scenario -- + std::cout << "[INFO] Running Radar scenario for vehicle: " << vehicle_name + << ", sensor: " << radar_name << std::endl; + if (!RunRadarScenario(client, world, vehicle_name, radar_name)) { + client->Disconnect(); + return 1; + } + + client->Disconnect(); + std::cout << "[OK] Radar scenario completed successfully" << std::endl; + return 0; + + } catch (const std::exception& ex) { + std::cerr << "[ERROR] Exception: " << ex.what() << std::endl; + return 1; + } catch (...) { + std::cerr << "[ERROR] Unknown exception" << std::endl; + return 1; + } +} diff --git a/client/cpp/example_user_apps/UserRoverScenario/UserRoverScenario.cpp b/client/cpp/example_user_apps/UserRoverScenario/UserRoverScenario.cpp new file mode 100644 index 00000000..a039b88e --- /dev/null +++ b/client/cpp/example_user_apps/UserRoverScenario/UserRoverScenario.cpp @@ -0,0 +1,260 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include +#include +#include + +namespace pasc = microsoft::projectairsim::client; +using pasc::Status; +using nlohmann::json; + +namespace { + +void LogSink(pasc::Log::Severity /*severity*/, const char* message) noexcept { + std::cout << message << std::endl; +} + +std::string StatusToString(Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +bool Ensure(Status status, const std::string& where) { + if (status == Status::OK) return true; + std::cerr << "[ERROR] " << where << ": " << StatusToString(status) << std::endl; + return false; +} + +void SleepSeconds(float seconds) { + std::this_thread::sleep_for( + std::chrono::duration_cast( + std::chrono::duration(seconds))); +} + +bool GetRoverPositionXY(pasc::Rover& rover, double* x_out, double* y_out) { + json kinematics; + if (!Ensure(rover.GetGroundTruthKinematics(&kinematics), + "GetGroundTruthKinematics")) + return false; + + try { + const auto& position = kinematics.at("pose").at("position"); + *x_out = position.at("x").get(); + *y_out = position.at("y").get(); + } catch (const std::exception& ex) { + std::cerr << "[ERROR] Could not parse rover position: " << ex.what() + << std::endl; + return false; + } + + return true; +} + +bool WaitUntilStopped(pasc::Rover& rover, float timeout_sec = 30.0f, + float speed_tolerance = 1.0e-5f) { + double x_last = 0.0; + double y_last = 0.0; + if (!GetRoverPositionXY(rover, &x_last, &y_last)) return false; + + const double speed_tolerance_sq = + static_cast(speed_tolerance) * speed_tolerance; + const auto start = std::chrono::steady_clock::now(); + auto last = start; + + while (true) { + SleepSeconds(0.1f); + + double x = 0.0; + double y = 0.0; + if (!GetRoverPositionXY(rover, &x, &y)) return false; + + const auto now = std::chrono::steady_clock::now(); + const double elapsed = + std::chrono::duration(now - start).count(); + if (elapsed >= timeout_sec) { + std::cerr << "[ERROR] Timed out waiting for rover to stop" << std::endl; + return false; + } + + const double dt = std::chrono::duration(now - last).count(); + const double dx = x - x_last; + const double dy = y - y_last; + const double dist_sq = dx * dx + dy * dy; + const double dist_tolerance_sq = speed_tolerance_sq * dt * dt; + if (dist_sq <= dist_tolerance_sq) return true; + + x_last = x; + y_last = y; + last = now; + } +} + +bool SetRoverControlsAndWait(pasc::Rover& rover, float engine, + float steering_angle, float brake, + float wait_sec, const std::string& label) { + std::cout << "[..] " << label << std::endl; + if (!Ensure(rover.SetRoverControls(engine, steering_angle, brake), + "SetRoverControls")) + return false; + SleepSeconds(wait_sec); + return true; +} + +bool BrakeRover(pasc::Rover& rover, float brake = 0.5f) { + std::cout << "[..] Braking" << std::endl; + if (!Ensure(rover.SetRoverControls(0.0f, 0.0f, brake), "SetRoverControls")) + return false; + return WaitUntilStopped(rover); +} + +bool RunRoverScenario(std::shared_ptr& client, + std::shared_ptr& world, + const std::string& vehicle_name) { + // -- Initialize rover -- + pasc::Rover rover; + if (!Ensure(rover.Initialize(client, world, vehicle_name), "Rover Initialize")) + return false; + std::cout << "[OK] Rover '" << vehicle_name << "' initialized" << std::endl; + + // -- Enable API control -- + bool api_enabled = false; + if (!Ensure(rover.EnableAPIControl(&api_enabled), "EnableAPIControl")) + return false; + std::cout << "[OK] API control enabled: " << std::boolalpha << api_enabled + << std::endl; + + // -- Arm -- + bool armed = false; + if (!Ensure(rover.Arm(&armed), "Arm")) return false; + std::cout << "[OK] Armed: " << armed << std::endl; + + if (!SetRoverControlsAndWait(rover, 0.0f, 0.0f, 0.0f, 5.0f, + "Setting rover controls to zero")) + return false; + + if (!SetRoverControlsAndWait(rover, 0.5f, 1.0f, 0.0f, 5.0f, + "Setting rover controls forward and to the right")) + return false; + + if (!SetRoverControlsAndWait(rover, 0.5f, -1.0f, 0.0f, 5.0f, + "Setting rover controls forward and to the left")) + return false; + + if (!BrakeRover(rover)) return false; + + if (!SetRoverControlsAndWait( + rover, -0.5f, 1.0f, 0.0f, 5.0f, + "Setting rover controls backwards and to the right")) + return false; + + if (!SetRoverControlsAndWait( + rover, -0.5f, -1.0f, 0.0f, 5.0f, + "Setting rover controls backwards and to the left")) + return false; + + if (!BrakeRover(rover)) return false; + + if (!SetRoverControlsAndWait(rover, 0.0f, 0.0f, 0.0f, 1.0f, + "Setting rover controls zero")) + return false; + + // -- Disarm -- + bool disarmed = false; + if (!Ensure(rover.Disarm(&disarmed), "Disarm")) return false; + std::cout << "[OK] Disarmed: " << disarmed << std::endl; + + bool api_disabled = false; + if (!Ensure(rover.DisableAPIControl(&api_disabled), "DisableAPIControl")) + return false; + std::cout << "[OK] API control disabled: " << api_disabled << std::endl; + + return true; +} + +} // namespace + +int main(int argc, const char* argv[]) { + std::string sim_host = "127.0.0.1"; + std::string sim_config = "client/python/example_user_scripts/sim_config"; + std::string scene_file = "scene_basic_rover.jsonc"; + std::string vehicle_name = "Rover1"; + + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (arg == "--simhost") { + if (++i >= argc) { + std::cerr << "Missing value for --simhost" << std::endl; + return 1; + } + sim_host = argv[i]; + } else if (arg == "--simconfig") { + if (++i >= argc) { + std::cerr << "Missing value for --simconfig" << std::endl; + return 1; + } + sim_config = argv[i]; + } else if (arg == "--scene") { + if (++i >= argc) { + std::cerr << "Missing value for --scene" << std::endl; + return 1; + } + scene_file = argv[i]; + } else if (arg == "--vehicle") { + if (++i >= argc) { + std::cerr << "Missing value for --vehicle" << std::endl; + return 1; + } + vehicle_name = argv[i]; + } else if (arg == "-h" || arg == "--help") { + std::cout << "user_rover_scenario [--simhost 127.0.0.1] [--simconfig " + "path] [--scene scene_basic_rover.jsonc] [--vehicle Rover1]" + << std::endl; + return 0; + } else { + std::cerr << "Unknown argument: " << arg << std::endl; + return 1; + } + } + + pasc::log.SetLogSink(LogSink); + + std::filesystem::path sim_path; + std::filesystem::path scene_path; + try { + sim_path = std::filesystem::canonical(sim_config); + scene_path = sim_path / scene_file; + } catch (const std::filesystem::filesystem_error& ex) { + std::cerr << "simconfig path error: " << ex.what() << std::endl; + return 1; + } + + auto client = std::make_shared(); + if (!Ensure(client->Connect(sim_host), "Client Connect")) return 1; + + auto world = std::make_shared(); + if (!Ensure(world->Initialize(client, scene_path.string(), sim_path.string(), + 2.0f), + "World Initialize")) { + client->Disconnect(); + return 1; + } + + const bool ok = RunRoverScenario(client, world, vehicle_name); + client->Disconnect(); + + if (!ok) { + std::cerr << "[FAIL] Rover scenario failed" << std::endl; + return 1; + } + + std::cout << "[PASS] Rover scenario completed" << std::endl; + return 0; +} diff --git a/client/cpp/example_user_apps/UserScenarioTemplate/UserScenarioTemplate.cpp b/client/cpp/example_user_apps/UserScenarioTemplate/UserScenarioTemplate.cpp new file mode 100644 index 00000000..e73508ac --- /dev/null +++ b/client/cpp/example_user_apps/UserScenarioTemplate/UserScenarioTemplate.cpp @@ -0,0 +1,140 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include + +namespace pasc = microsoft::projectairsim::client; +using pasc::Status; + +namespace { + +void LogSink(pasc::Log::Severity /*severity*/, const char* message) noexcept { + std::cout << message << std::endl; +} + +std::string StatusToString(Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +bool Ensure(Status status, const std::string& where) { + if (status == Status::OK) return true; + std::cerr << "[ERROR] " << where << ": " << StatusToString(status) << std::endl; + return false; +} + +bool RunUserScenario(std::shared_ptr& client, std::shared_ptr& world, + const std::string& vehicle_name) { + auto drone = std::make_shared(); + if (!Ensure(drone->Initialize(client, world, vehicle_name), "Initialize " + vehicle_name)) { + return false; + } + + bool state = false; + if (!Ensure(drone->EnableAPIControl(&state), vehicle_name + " EnableAPIControl")) return false; + if (!Ensure(drone->Arm(&state), vehicle_name + " Arm")) return false; + + auto takeoff = drone->TakeoffAsync(); + if (!Ensure(takeoff.Wait(), vehicle_name + " TakeoffAsync")) return false; + + // Template action: move up for 3 seconds. Replace this block with custom logic. + auto move = drone->MoveByVelocityAsync(0.0f, 0.0f, -1.0f, 3.0f); + if (!Ensure(move.Wait(), vehicle_name + " MoveByVelocityAsync")) return false; + + auto land = drone->LandAsync(); + if (!Ensure(land.Wait(), vehicle_name + " LandAsync")) return false; + + if (!Ensure(drone->Disarm(&state), vehicle_name + " Disarm")) return false; + if (!Ensure(drone->DisableAPIControl(&state), vehicle_name + " DisableAPIControl")) { + return false; + } + + return true; +} + +} // namespace + +int main(int argc, const char* argv[]) { + std::string sim_host = "127.0.0.1"; + std::string sim_config = "client/python/example_user_scripts/sim_config"; + std::string scene_file = "scene_basic_drone.jsonc"; + std::string vehicle_name = "Drone1"; + + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (arg == "--simhost") { + if (++i >= argc) { + std::cerr << "Missing value for --simhost" << std::endl; + return 1; + } + sim_host = argv[i]; + } else if (arg == "--simconfig") { + if (++i >= argc) { + std::cerr << "Missing value for --simconfig" << std::endl; + return 1; + } + sim_config = argv[i]; + } else if (arg == "--scene") { + if (++i >= argc) { + std::cerr << "Missing value for --scene" << std::endl; + return 1; + } + scene_file = argv[i]; + } else if (arg == "--vehicle") { + if (++i >= argc) { + std::cerr << "Missing value for --vehicle" << std::endl; + return 1; + } + vehicle_name = argv[i]; + } else if (arg == "-h" || arg == "--help") { + std::cout + << "user_scenario_template [--simhost 127.0.0.1] [--simconfig path] " + "[--scene scene_basic_drone.jsonc] [--vehicle Drone1]" + << std::endl; + return 0; + } else { + std::cerr << "Unknown argument: " << arg << std::endl; + return 1; + } + } + + pasc::log.SetLogSink(LogSink); + + std::filesystem::path sim_path; + std::filesystem::path scene_path; + try { + sim_path = std::filesystem::canonical(sim_config); + scene_path = sim_path / scene_file; + } catch (const std::filesystem::filesystem_error& ex) { + std::cerr << "simconfig path error: " << ex.what() << std::endl; + return 1; + } + + auto client = std::make_shared(); + if (!Ensure(client->Connect(sim_host), "Client Connect")) return 1; + + auto world = std::make_shared(); + if (!Ensure(world->Initialize(client, scene_path.string(), sim_path.string(), 2.0f), + "World Initialize")) { + client->Disconnect(); + return 1; + } + + const bool ok = RunUserScenario(client, world, vehicle_name); + client->Disconnect(); + + if (!ok) { + std::cerr << "[FAIL] User scenario failed" << std::endl; + return 1; + } + + std::cout << "[PASS] User scenario completed" << std::endl; + return 0; +} diff --git a/client/cpp/example_user_apps/UserStaticSensorScenario/UserStaticSensorScenario.cpp b/client/cpp/example_user_apps/UserStaticSensorScenario/UserStaticSensorScenario.cpp new file mode 100644 index 00000000..4ad1181e --- /dev/null +++ b/client/cpp/example_user_apps/UserStaticSensorScenario/UserStaticSensorScenario.cpp @@ -0,0 +1,144 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include +#include + +namespace pasc = microsoft::projectairsim::client; +using pasc::Status; +using nlohmann::json; + +namespace { + +void LogSink(pasc::Log::Severity /*severity*/, const char* message) noexcept { + std::cout << message << std::endl; +} + +std::string StatusToString(Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +bool Ensure(Status status, const std::string& where) { + if (status == Status::OK) return true; + std::cerr << "[ERROR] " << where << ": " << StatusToString(status) + << std::endl; + return false; +} + +bool RunStaticSensorScenario(std::shared_ptr& client, + std::shared_ptr& world, + const std::string& actor_name, + const std::string& camera_name) { + pasc::StaticSensorActor actor; + if (!Ensure(actor.Initialize(client, world, actor_name), + "StaticSensorActor Initialize")) + return false; + + json images; + if (!Ensure(actor.GetImages(camera_name, {0}, &images), "GetImages")) + return false; + + std::cout << "[OK] Retrieved images from camera '" << camera_name << "'" + << std::endl; + std::cout << "[OK] Image payload keys: " << images.size() << std::endl; + + return true; +} + +} // namespace + +int main(int argc, const char* argv[]) { + std::string sim_host = "127.0.0.1"; + std::string sim_config = "client/python/example_user_scripts/sim_config"; + std::string scene_file = "scene_computer_vision.jsonc"; + std::string actor_name = "CV"; + std::string camera_name = "Camera"; + + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (arg == "--simhost") { + if (++i >= argc) { + std::cerr << "Missing value for --simhost" << std::endl; + return 1; + } + sim_host = argv[i]; + } else if (arg == "--simconfig") { + if (++i >= argc) { + std::cerr << "Missing value for --simconfig" << std::endl; + return 1; + } + sim_config = argv[i]; + } else if (arg == "--scene") { + if (++i >= argc) { + std::cerr << "Missing value for --scene" << std::endl; + return 1; + } + scene_file = argv[i]; + } else if (arg == "--vehicle" || arg == "--actor") { + if (++i >= argc) { + std::cerr << "Missing value for --vehicle/--actor" << std::endl; + return 1; + } + actor_name = argv[i]; + } else if (arg == "--camera") { + if (++i >= argc) { + std::cerr << "Missing value for --camera" << std::endl; + return 1; + } + camera_name = argv[i]; + } else if (arg == "-h" || arg == "--help") { + std::cout + << "user_static_sensor_scenario [--simhost 127.0.0.1] [--simconfig " + "path] [--scene scene_computer_vision.jsonc] [--vehicle CV] " + "[--camera Camera]" + << std::endl; + return 0; + } else { + std::cerr << "Unknown argument: " << arg << std::endl; + return 1; + } + } + + pasc::log.SetLogSink(LogSink); + + std::filesystem::path sim_path; + std::filesystem::path scene_path; + try { + sim_path = std::filesystem::canonical(sim_config); + scene_path = sim_path / scene_file; + } catch (const std::filesystem::filesystem_error& ex) { + std::cerr << "simconfig path error: " << ex.what() << std::endl; + return 1; + } + + auto client = std::make_shared(); + if (!Ensure(client->Connect(sim_host), "Client Connect")) return 1; + + auto world = std::make_shared(); + if (!Ensure(world->Initialize(client, scene_path.string(), sim_path.string(), + 2.0f), + "World Initialize")) { + client->Disconnect(); + return 1; + } + + const bool ok = + RunStaticSensorScenario(client, world, actor_name, camera_name); + client->Disconnect(); + + if (!ok) { + std::cerr << "[FAIL] StaticSensor scenario failed" << std::endl; + return 1; + } + + std::cout << "[PASS] StaticSensor scenario completed" << std::endl; + return 0; +} diff --git a/client/cpp/packaging/ProjectAirSimCppClientConfig.cmake.in b/client/cpp/packaging/ProjectAirSimCppClientConfig.cmake.in new file mode 100644 index 00000000..7d0f82c9 --- /dev/null +++ b/client/cpp/packaging/ProjectAirSimCppClientConfig.cmake.in @@ -0,0 +1,38 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +if(NOT WIN32) + find_dependency(Threads) +endif() + +set(ProjectAirSimCppClient_VERSION "@PROJECT_VERSION@") +set(ProjectAirSimCppClient_PLATFORM "@PROJECTAIRSIM_CPP_CLIENT_PACKAGE_PLATFORM@") +set(ProjectAirSimCppClient_ROOT_DIR "${PACKAGE_PREFIX_DIR}") +set(ProjectAirSimCppClient_INCLUDE_DIR "${PACKAGE_PREFIX_DIR}/include") +set(ProjectAirSimCppClient_LIB_DIR "${PACKAGE_PREFIX_DIR}/lib") +set(ProjectAirSimCppClient_BIN_DIR "${PACKAGE_PREFIX_DIR}/bin") + +set(_projectairsim_cpp_client_lib_name + "@PROJECTAIRSIM_CPP_CLIENT_LIB_PREFIX@ProjectAirSimCppClient@PROJECTAIRSIM_CPP_CLIENT_LIB_SUFFIX@" +) + +set(_projectairsim_cpp_client_link_libs) + +if(WIN32) + list(APPEND _projectairsim_cpp_client_link_libs Version ws2_32 mswsock advapi32) +else() + list(APPEND _projectairsim_cpp_client_link_libs Threads::Threads) +endif() + +if(NOT TARGET ProjectAirSim::Client) + add_library(ProjectAirSim::Client STATIC IMPORTED) + set_target_properties(ProjectAirSim::Client PROPERTIES + IMPORTED_LOCATION "${ProjectAirSimCppClient_LIB_DIR}/${_projectairsim_cpp_client_lib_name}" + INTERFACE_INCLUDE_DIRECTORIES "${ProjectAirSimCppClient_INCLUDE_DIR}" + INTERFACE_COMPILE_DEFINITIONS "PROJECTAIRSIMCLIENT_STATIC;NNGI_STATIC_LIB;NNG_STATIC_LIB;MSGPACK_NO_BOOST" + INTERFACE_LINK_LIBRARIES "${_projectairsim_cpp_client_link_libs}" + ) +endif() + +check_required_components(ProjectAirSimCppClient) diff --git a/client/cpp/packaging/README.md.in b/client/cpp/packaging/README.md.in new file mode 100644 index 00000000..68e04d46 --- /dev/null +++ b/client/cpp/packaging/README.md.in @@ -0,0 +1,65 @@ +# Project AirSim C++ Client @PROJECT_VERSION@ + +Native C++17 client package for Project AirSim. + +## Package Contents + +```text +bin/ Example executables +include/ Public headers and bundled third-party headers +lib/ Static libraries +lib/cmake/ProjectAirSimCppClient/ + CMake package files +examples/ Example source files and sample sim_config files +scripts/ Helper scripts +license.md License +``` + +## Libraries + +The package exposes one final static library: + +- `ProjectAirSimCppClient` + +It contains the high-level client, message library, NNGI wrapper, and NNG static +library. On Windows, consumers also link the Windows SDK/system libraries +declared by the CMake package, including `Version`, `ws2_32`, `mswsock`, and +`advapi32`. + +## Use From CMake + +Point `CMAKE_PREFIX_PATH` at the package root, or pass +`-DProjectAirSimCppClient_DIR=/lib/cmake/ProjectAirSimCppClient`. + +```cmake +cmake_minimum_required(VERSION 3.20) +project(MyProjectAirSimApp LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ProjectAirSimCppClient REQUIRED) + +add_executable(my_app main.cpp) +target_link_libraries(my_app PRIVATE ProjectAirSim::Client) +``` + +## Run HelloDrone + +Start a Project AirSim simulation server first, then run: + +```bash +bin/hello_drone --simhost 127.0.0.1 --simconfig examples/sim_config +``` + +On Windows: + +```bat +bin\hello_drone.exe --simhost 127.0.0.1 --simconfig examples\sim_config +``` + +## Build Metadata + +- Version: `@PROJECT_VERSION@` +- Platform package: `@PROJECTAIRSIM_CPP_CLIENT_PACKAGE_PLATFORM@` +- Build type: `@CMAKE_BUILD_TYPE@` diff --git a/client/cpp/scripts/run_cpp_client_scenario_basic.sh b/client/cpp/scripts/run_cpp_client_scenario_basic.sh new file mode 100755 index 00000000..58da12c4 --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_basic.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +"$SCRIPT_DIR/run_cpp_client_scenarios.sh" --only basic "$@" diff --git a/client/cpp/scripts/run_cpp_client_scenario_battery.sh b/client/cpp/scripts/run_cpp_client_scenario_battery.sh new file mode 100755 index 00000000..91d007f8 --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_battery.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +"$SCRIPT_DIR/run_cpp_client_scenarios.sh" --only battery "$@" diff --git a/client/cpp/scripts/run_cpp_client_scenario_env_actor.sh b/client/cpp/scripts/run_cpp_client_scenario_env_actor.sh new file mode 100755 index 00000000..3593768e --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_env_actor.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +"$SCRIPT_DIR/run_cpp_user_scenario.sh" \ + --target user_env_actor_scenario \ + --scene scene_env_actor.jsonc \ + "$@" diff --git a/client/cpp/scripts/run_cpp_client_scenario_lidar.sh b/client/cpp/scripts/run_cpp_client_scenario_lidar.sh new file mode 100644 index 00000000..882c15cb --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_lidar.sh @@ -0,0 +1,55 @@ +#!/bin/bash +# Run the Lidar scenario + +set -e + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +REPO_ROOT="$(cd "$SCRIPT_DIR/../.." && pwd)" + +SIMHOST="127.0.0.1" +SIMCONFIG="$REPO_ROOT/client/python/example_user_scripts/sim_config" +SCENE="scene_lidar_drone.jsonc" +VEHICLE="Drone1" +LIDAR_NAME="Lidar1" + +# Parse arguments +while [[ $# -gt 0 ]]; do + case $1 in + --simhost) + SIMHOST="$2" + shift 2 + ;; + --simconfig) + SIMCONFIG="$2" + shift 2 + ;; + --scene) + SCENE="$2" + shift 2 + ;; + --vehicle) + VEHICLE="$2" + shift 2 + ;; + --lidar) + LIDAR_NAME="$2" + shift 2 + ;; + *) + echo "Unknown option: $1" + exit 1 + ;; + esac +done + +echo "[INFO] Running Lidar scenario" +echo "[INFO] Sim host: $SIMHOST" +echo "[INFO] Vehicle: $VEHICLE, Lidar: $LIDAR_NAME" + +exec "$SCRIPT_DIR/run_cpp_user_scenario.sh" \ + --target user_lidar_scenario \ + --simhost "$SIMHOST" \ + --simconfig "$SIMCONFIG" \ + --scene "$SCENE" \ + --vehicle "$VEHICLE" \ + --lidar "$LIDAR_NAME" diff --git a/client/cpp/scripts/run_cpp_client_scenario_radar.sh b/client/cpp/scripts/run_cpp_client_scenario_radar.sh new file mode 100644 index 00000000..84c9dd66 --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_radar.sh @@ -0,0 +1,55 @@ +#!/bin/bash +# Run the Radar scenario + +set -e + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +REPO_ROOT="$(cd "$SCRIPT_DIR/../.." && pwd)" + +SIMHOST="127.0.0.1" +SIMCONFIG="$REPO_ROOT/client/python/example_user_scripts/sim_config" +SCENE="scene_radar_tower.jsonc" +VEHICLE="RadarTower1" +RADAR_NAME="radar1" + +# Parse arguments +while [[ $# -gt 0 ]]; do + case $1 in + --simhost) + SIMHOST="$2" + shift 2 + ;; + --simconfig) + SIMCONFIG="$2" + shift 2 + ;; + --scene) + SCENE="$2" + shift 2 + ;; + --vehicle) + VEHICLE="$2" + shift 2 + ;; + --radar) + RADAR_NAME="$2" + shift 2 + ;; + *) + echo "Unknown option: $1" + exit 1 + ;; + esac +done + +echo "[INFO] Running Radar scenario" +echo "[INFO] Sim host: $SIMHOST" +echo "[INFO] Vehicle: $VEHICLE, Radar: $RADAR_NAME" + +exec "$SCRIPT_DIR/run_cpp_user_scenario.sh" \ + --target user_radar_scenario \ + --simhost "$SIMHOST" \ + --simconfig "$SIMCONFIG" \ + --scene "$SCENE" \ + --vehicle "$VEHICLE" \ + --radar "$RADAR_NAME" diff --git a/client/cpp/scripts/run_cpp_client_scenario_sensors.sh b/client/cpp/scripts/run_cpp_client_scenario_sensors.sh new file mode 100755 index 00000000..b014aebe --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_sensors.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +"$SCRIPT_DIR/run_cpp_client_scenarios.sh" --only sensors "$@" diff --git a/client/cpp/scripts/run_cpp_client_scenario_static_sensor.sh b/client/cpp/scripts/run_cpp_client_scenario_static_sensor.sh new file mode 100755 index 00000000..39ec59c1 --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_static_sensor.sh @@ -0,0 +1,9 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +"$SCRIPT_DIR/run_cpp_user_scenario.sh" \ + --target user_static_sensor_scenario \ + --scene scene_computer_vision.jsonc \ + --vehicle CV \ + "$@" diff --git a/client/cpp/scripts/run_cpp_client_scenario_two_drones.sh b/client/cpp/scripts/run_cpp_client_scenario_two_drones.sh new file mode 100755 index 00000000..eec3d765 --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_two_drones.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +"$SCRIPT_DIR/run_cpp_client_scenarios.sh" --only two_drones "$@" diff --git a/client/cpp/scripts/run_cpp_client_scenario_wind.sh b/client/cpp/scripts/run_cpp_client_scenario_wind.sh new file mode 100755 index 00000000..a12acb9e --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenario_wind.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +"$SCRIPT_DIR/run_cpp_client_scenarios.sh" --only wind "$@" diff --git a/client/cpp/scripts/run_cpp_client_scenarios.sh b/client/cpp/scripts/run_cpp_client_scenarios.sh new file mode 100755 index 00000000..bc4a24bc --- /dev/null +++ b/client/cpp/scripts/run_cpp_client_scenarios.sh @@ -0,0 +1,159 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)" +BUILD_DIR="$ROOT_DIR/client/cpp/build_local" +BIN="$BUILD_DIR/cpp_client_scenarios" + +SIMHOST="127.0.0.1" +SIMCONFIG="$ROOT_DIR/client/python/example_user_scripts/sim_config" +ONLY="" + +usage() { + cat < + +#include +#include + +namespace asc = microsoft::projectairsim::client; + +namespace { + +std::string StatusToString(asc::Status status) { + char buffer[256] = {0}; + asc::GetStatusString(status, buffer); + return std::string(buffer); +} + +} // namespace + +int main(int argc, char** argv) { + std::string host = "127.0.0.1"; + if (argc > 1) host = argv[1]; + + asc::Client client; + const auto status = client.Connect(host); + if (status != asc::Status::OK) { + std::cerr << "[FAIL] Client::Connect(" << host << ") failed: " + << StatusToString(status) << std::endl; + std::cerr << "[INFO] Start Unreal/ProjectAirSim simserver before running " + "this integration test." + << std::endl; + return 1; + } + + std::cout << "[OK] Client::Connect(" << host + << ") succeeded against real NNG server" << std::endl; + client.Disconnect(); + return 0; +} + diff --git a/client/cpp/tests/CppClientUnitTests.cpp b/client/cpp/tests/CppClientUnitTests.cpp new file mode 100644 index 00000000..7972c0ae --- /dev/null +++ b/client/cpp/tests/CppClientUnitTests.cpp @@ -0,0 +1,1554 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "msgpack.hpp" + +#include "FakeNNGI.h" + +namespace asc = microsoft::projectairsim::client; +namespace pas = microsoft::projectairsim; +namespace fake_nngi = projectairsim_cpp_tests::fake_nngi; +using json = nlohmann::json; + +namespace { + +bool Expect(bool condition, const std::string& message) { + if (!condition) { + std::cerr << "[FAIL] " << message << std::endl; + return false; + } + return true; +} + +std::string SuccessResponse(int id, const json& result) { + return pas::ResponseMessage(id, result, asc::kClientAPIVersion).Serialize(); +} + +std::string ErrorResponse(int id, const std::string& message) { + return pas::ResponseMessage(id, 1, message, asc::kClientAPIVersion) + .Serialize(); +} + +pas::RequestMessage ParseRequest(const std::string& raw) { + pas::RequestMessage request; + request.Deserialize(raw); + return request; +} + +void PackTopicInfo(msgpack::packer* packer, + const std::string& path) { + packer->pack_map(4); + packer->pack(std::string("path")); + packer->pack(path); + packer->pack(std::string("type")); + packer->pack(std::string("topic")); + packer->pack(std::string("message_type")); + packer->pack(std::string("msgpack")); + packer->pack(std::string("frequency")); + packer->pack(10); +} + +std::string TopicListFrame(const std::vector& topics) { + msgpack::sbuffer body; + msgpack::packer body_packer(body); + body_packer.pack_array(topics.size()); + for (const auto& topic : topics) PackTopicInfo(&body_packer, topic); + + msgpack::sbuffer frame; + msgpack::packer frame_packer(frame); + frame_packer.pack_array(3); + frame_packer.pack(2); + frame_packer.pack(std::string("/$topics")); + frame_packer.pack(std::string(body.data(), body.size())); + return std::string(frame.data(), frame.size()); +} + +std::vector BasicTopics() { + return { + "/Sim/SceneUnit/robots/Drone1/actual_pose", + "/Sim/SceneUnit/robots/Drone1/collision_info", + "/Sim/SceneUnit/robots/Drone1/rotor_info", + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/scene_camera", + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/depth_camera", + "/Sim/SceneUnit/robots/Drone1/sensors/IMU1/imu_kinematics", + "/Sim/SceneUnit/robots/Drone1/sensors/GPS/gps", + "/Sim/SceneUnit/robots/Drone1/sensors/Barometer/barometer", + "/Sim/SceneUnit/robots/Drone1/sensors/Magnetometer/magnetometer", + "/Sim/SceneUnit/robots/Drone1/sensors/Airspeed/airspeed", + "/Sim/SceneUnit/robots/Drone1/sensors/Battery/battery", + "/Sim/SceneUnit/robots/Drone1/sensors/Radar1/radar_detections", + "/Sim/SceneUnit/robots/Drone1/sensors/Radar1/radar_tracks", + "/Sim/SceneUnit/robots/Drone1/sensors/Lidar1/lidar", + }; +} + +struct TestEnvironment { + std::shared_ptr client; + std::shared_ptr world; + std::shared_ptr drone; + std::shared_ptr rover; + std::shared_ptr env_actor; + std::shared_ptr static_sensor_actor; +}; + +bool CreateWorld(TestEnvironment* env) { + fake_nngi::Reset(); + fake_nngi::PushTopicMessage(TopicListFrame(BasicTopics())); + + env->client = std::make_shared(); + env->world = std::make_shared(); + + if (!Expect(env->client->Connect("127.0.0.1") == asc::Status::OK, + "Client::Connect should succeed")) { + return false; + } + if (!Expect(env->world->Initialize(env->client) == asc::Status::OK, + "World::Initialize should derive scene from topic info")) { + return false; + } + return true; +} + +bool CreateDrone(TestEnvironment* env) { + if (!CreateWorld(env)) return false; + + env->drone = std::make_shared(); + if (!Expect(env->drone->Initialize(env->client, env->world, "Drone1") == + asc::Status::OK, + "Drone::Initialize should derive sensor topics from topic info")) { + return false; + } + return true; +} + +bool CreateRover(TestEnvironment* env) { + if (!CreateWorld(env)) return false; + + env->rover = std::make_shared(); + if (!Expect(env->rover->Initialize(env->client, env->world, "Rover1") == + asc::Status::OK, + "Rover::Initialize should attach to the world topic")) { + return false; + } + return true; +} + +bool CreateEnvActor(TestEnvironment* env) { + if (!CreateWorld(env)) return false; + + env->env_actor = std::make_shared(); + if (!Expect(env->env_actor->Initialize(env->client, env->world, "Actor1") == + asc::Status::OK, + "EnvActor::Initialize should attach to the world topic")) { + return false; + } + return true; +} + +bool CreateStaticSensorActor(TestEnvironment* env) { + if (!CreateWorld(env)) return false; + + env->static_sensor_actor = std::make_shared(); + if (!Expect(env->static_sensor_actor->Initialize(env->client, env->world, + "Static1") == + asc::Status::OK, + "StaticSensorActor::Initialize should attach to the world topic")) { + return false; + } + return true; +} + +json TransformResultJson() { + return json{{"timestamp", 123}, + {"frame_id", "world"}, + {"translation", {{"x", 1.0f}, {"y", 2.0f}, {"z", 3.0f}}}, + {"rotation", {{"w", 1.0f}, {"x", 0.0f}, {"y", 0.0f}, {"z", 0.0f}}}}; +} + +json GeoPositionResultJson() { + return json{{"latitude", 47.0f}, {"longitude", -122.0f}, {"altitude", 10.0f}}; +} + +bool ExpectLastRequest(const std::string& method, + const std::function& check_params, + const std::string& message) { + auto sent = fake_nngi::SentServiceMessages(); + if (!Expect(!sent.empty(), message + ": should send a service request")) { + return false; + } + + const auto request = ParseRequest(sent.back()); + if (!Expect(request.GetMethod() == method, + message + ": method should be " + method)) { + return false; + } + if (!Expect(check_params(request.GetParams()), + message + ": params should match")) { + return false; + } + return true; +} + +bool ExpectLastRequest(const std::string& method, const std::string& message) { + return ExpectLastRequest(method, [](const json&) { return true; }, message); +} + +bool TestConnectAndRequest() { + fake_nngi::Reset(); + fake_nngi::PushServiceResponse(SuccessResponse(0, {{"ok", true}})); + + asc::Client client; + if (!Expect(client.Connect("127.0.0.1") == asc::Status::OK, + "Client::Connect should succeed with fake NNG")) { + return false; + } + + auto dialed_urls = fake_nngi::DialedUrls(); + if (!Expect(dialed_urls.size() == 2, "Connect should dial services and topics")) return false; + if (!Expect(dialed_urls[0] == "tcp://127.0.0.1:8990", + "Connect should dial services port 8990")) { + return false; + } + if (!Expect(dialed_urls[1] == "tcp://127.0.0.1:8989", + "Connect should dial topics port 8989")) { + return false; + } + + asc::Message response; + const auto status = + client.Request("/Sim/Unit", {{"speed", 3.0}, {"name", "Drone1"}}, &response); + if (!Expect(status == asc::Status::OK, "Client::Request should receive fake response")) { + return false; + } + + auto sent = fake_nngi::SentServiceMessages(); + if (!Expect(sent.size() == 1, "Request should send one service message")) return false; + + const auto request = ParseRequest(sent[0]); + if (!Expect(request.GetID() == 0, "Request id should be 0")) return false; + if (!Expect(request.GetMethod() == "/Sim/Unit", "Request method should match")) return false; + if (!Expect(request.GetParams()["speed"] == 3.0, "Request params should include speed")) { + return false; + } + if (!Expect(request.GetParams()["name"] == "Drone1", "Request params should include name")) { + return false; + } + + pas::ResponseMessage response_message; + response_message.Deserialize(static_cast(response)); + if (!Expect(response_message.GetErrorCode() == 0, "Response should be success")) return false; + if (!Expect(response_message.GetResult()["ok"] == true, "Response result should be preserved")) { + return false; + } + + client.Disconnect(); + return true; +} + +bool TestClientPublishSendsTopicMessage() { + fake_nngi::Reset(); + fake_nngi::PushTopicMessage(TopicListFrame(BasicTopics())); + + asc::Client client; + if (!Expect(client.Connect("127.0.0.1") == asc::Status::OK, + "Client::Connect should succeed")) { + return false; + } + + if (!Expect(client.Publish("/Sim/Topic", {{"value", 42}}) == asc::Status::OK, + "Client::Publish should succeed")) { + return false; + } + + const auto sent_topics = fake_nngi::SentTopicMessages(); + if (!Expect(sent_topics.size() >= 2, + "Publish should send topic subscribe and message frames")) { + return false; + } + + auto object_handle = + msgpack::unpack(sent_topics.back().data(), sent_topics.back().size()); + msgpack::object frame = object_handle.get(); + std::tuple unpacked; + frame.convert(unpacked); + + if (!Expect(std::get<0>(unpacked) == 2, "Publish frame type should be message")) { + return false; + } + if (!Expect(std::get<1>(unpacked) == "/Sim/Topic", + "Publish topic should match")) { + return false; + } + + client.Disconnect(); + return true; +} + +bool TestClientTopicAndLifecycleAPIsUseExpectedMessages() { + fake_nngi::Reset(); + fake_nngi::PushTopicMessage(TopicListFrame(BasicTopics())); + + asc::Client client; + if (!Expect(client.Connect("127.0.0.1") == asc::Status::OK, + "Client::Connect should succeed")) { + return false; + } + + const auto topic_info = client.GetTopicInfo(); + if (!Expect(!topic_info.empty(), "Client::GetTopicInfo should return topic paths") || + !Expect(topic_info[0] == "/Sim/SceneUnit/robots/Drone1/actual_pose", + "Client::GetTopicInfo should preserve topic path")) { + return false; + } + + if (!Expect(client.Subscribe("/Sim/Topic", + [](const std::string&, const std::string&) {}) == + asc::Status::OK, + "Client::Subscribe should succeed")) { + return false; + } + auto sent_topics = fake_nngi::SentTopicMessages(); + if (!Expect(!sent_topics.empty(), "Client::Subscribe should send a topic frame")) { + return false; + } + + fake_nngi::PushServiceResponse(SuccessResponse(0, true)); + if (!Expect(client.Unsubscribe(std::vector{"/Sim/Topic"}) == + asc::Status::OK, + "Client::Unsubscribe vector should succeed")) { + return false; + } + if (!ExpectLastRequest( + "/Sim/Unsubscribe", + [](const json& p) { + return p["topic_paths"].size() == 1 && + p["topic_paths"][0] == "/Sim/Topic"; + }, + "Client::Unsubscribe vector")) { + return false; + } + + if (!Expect(client.Subscribe("/Sim/Topic2", + [](const std::string&, const std::string&) {}) == + asc::Status::OK, + "Client::Subscribe should succeed before UnsubscribeAll")) { + return false; + } + fake_nngi::PushServiceResponse(SuccessResponse(1, true)); + if (!Expect(client.UnsubscribeAll() == asc::Status::OK, + "Client::UnsubscribeAll should succeed")) { + return false; + } + if (!ExpectLastRequest("/Sim/Unsubscribe", + [](const json& p) { + return p["topic_paths"].size() == 1 && + p["topic_paths"][0] == "/Sim/Topic2"; + }, + "Client::UnsubscribeAll")) { + return false; + } + + if (!Expect(client.Subscribe("/Sim/Topic3", + [](const std::string&, const std::string&) {}) == + asc::Status::OK, + "Client::Subscribe should succeed before char unsubscribe")) { + return false; + } + if (!Expect(client.Unsubscribe("/Sim/Topic3") == asc::Status::OK, + "Client::Unsubscribe char should succeed")) { + return false; + } + if (!Expect(client.UnsubscribeAll() == asc::Status::OK, + "Client::UnsubscribeAll should be OK after char unsubscribe")) { + return false; + } + + bool enabled = false; + fake_nngi::PushServiceResponse(SuccessResponse(2, true)); + if (!Expect(client.SetInteractiveFeature("weather", true, &enabled) == + asc::Status::OK, + "Client::SetInteractiveFeature should succeed") || + !Expect(enabled, "Client::SetInteractiveFeature should parse bool") || + !ExpectLastRequest( + "/Sim/SetInteractiveFeature", + [](const json& p) { + return p["feature_id"] == "weather" && p["enable"] == true; + }, + "Client::SetInteractiveFeature")) { + return false; + } + + client.Disconnect(); + asc::Message response; + return Expect(client.Request("/Sim/Unit", json::object(), &response) == + asc::Status::NotConnected, + "Client::Disconnect should close service requests"); +} + +bool TestRequestBeforeConnectFails() { + fake_nngi::Reset(); + + asc::Client client; + asc::Message response; + const auto status = client.Request("/Sim/Unit", json::object(), &response); + return Expect(status == asc::Status::NotConnected, + "Request before Connect should return NotConnected"); +} + +bool TestRejectedResponseReturnsRejectedByServer() { + TestEnvironment env; + if (!CreateWorld(&env)) return false; + + fake_nngi::PushServiceResponse(ErrorResponse(0, "unit failure")); + std::string clock_type; + const auto status = env.world->GetSimClockType(&clock_type); + env.client->Disconnect(); + return Expect(status == asc::Status::RejectedByServer, + "World wrapper server error should return RejectedByServer"); +} + +bool TestGetBuildCommitHashUsesExpectedServiceMethod() { + fake_nngi::Reset(); + fake_nngi::PushServiceResponse(SuccessResponse(0, "abc123")); + + asc::Client client; + if (!Expect(client.Connect("localhost") == asc::Status::OK, + "Client::Connect should succeed")) { + return false; + } + + std::string hash; + if (!Expect(client.GetBuildCommitHash(&hash) == asc::Status::OK, + "GetBuildCommitHash should succeed")) { + return false; + } + if (!Expect(hash == "abc123", "GetBuildCommitHash should parse string result")) return false; + + auto sent = fake_nngi::SentServiceMessages(); + if (!Expect(sent.size() == 1, "GetBuildCommitHash should send one request")) return false; + const auto request = ParseRequest(sent[0]); + if (!Expect(request.GetMethod() == "/Sim/GetBuildCommitHash", + "GetBuildCommitHash should use expected service method")) { + return false; + } + + client.Disconnect(); + return true; +} + +bool TestWorldAPIsUseExpectedServiceMethods() { + TestEnvironment env; + if (!CreateWorld(&env)) return false; + + int id = 0; + auto push = [&](const json& result) { + fake_nngi::PushServiceResponse(SuccessResponse(id++, result)); + }; + + std::string str_out; + std::int64_t time_out = 0; + bool bool_out = false; + float float_out = 0.0f; + asc::Vector3 vector_out; + asc::VecStr names_out; + std::vector scale_out; + json json_out; + + push("steppable"); + if (!Expect(env.world->GetSimClockType(&str_out) == asc::Status::OK, + "World::GetSimClockType should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/GetSimClockType", + "World::GetSimClockType")) { + return false; + } + + push(1234); + if (!Expect(env.world->GetSimTime(&time_out) == asc::Status::OK, + "World::GetSimTime should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/GetSimTime", "World::GetSimTime")) { + return false; + } + + push("paused"); + if (!Expect(env.world->Pause(&str_out) == asc::Status::OK, + "World::Pause should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/Pause", + [](const json& p) { return p["do_pause"] == true; }, + "World::Pause")) { + return false; + } + + push("running"); + if (!Expect(env.world->Resume(&str_out) == asc::Status::OK, + "World::Resume should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/Pause", + [](const json& p) { return p["do_pause"] == false; }, + "World::Resume")) { + return false; + } + + push(true); + if (!Expect(env.world->IsPaused(&bool_out) == asc::Status::OK, + "World::IsPaused should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/IsPaused", "World::IsPaused")) { + return false; + } + + push(2000); + if (!Expect(env.world->ContinueForSimTime(100, true, &time_out) == + asc::Status::OK, + "World::ContinueForSimTime should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/ContinueForSimTime", + [](const json& p) { + return p["delta_time"] == 100 && + p["wait_until_complete"] == true; + }, + "World::ContinueForSimTime")) { + return false; + } + + push(3000); + if (!Expect(env.world->ContinueUntilSimTime(3000, false, &time_out) == + asc::Status::OK, + "World::ContinueUntilSimTime should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/ContinueUntilSimTime", + [](const json& p) { + return p["target_time"] == 3000 && + p["wait_until_complete"] == false; + }, + "World::ContinueUntilSimTime")) { + return false; + } + + push(4000); + if (!Expect(env.world->ContinueForNSteps(4, true, &time_out) == + asc::Status::OK, + "World::ContinueForNSteps should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/ContinueForNSteps", + [](const json& p) { + return p["n_steps"] == 4 && + p["wait_until_complete"] == true; + }, + "World::ContinueForNSteps")) { + return false; + } + + push(5000); + if (!Expect(env.world->ContinueForSingleStep(false, &time_out) == + asc::Status::OK, + "World::ContinueForSingleStep should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/ContinueForSingleStep", + [](const json& p) { + return p["wait_until_complete"] == false; + }, + "World::ContinueForSingleStep")) { + return false; + } + + struct BoolCase { + std::string name; + std::function call; + std::string method; + std::function params; + }; + + const std::vector bool_cases = { + {"SetSunlightIntensity", + [&]() { return env.world->SetSunlightIntensity(10.0f, &bool_out); }, + "/Sim/SceneUnit/SetSunLightIntensity", + [](const json& p) { return p["intensity"] == 10.0f; }}, + {"SetCloudShadowStrength", + [&]() { return env.world->SetCloudShadowStrength(0.5f, &bool_out); }, + "/Sim/SceneUnit/SetCloudShadowStrength", + [](const json& p) { return p["strength"] == 0.5f; }}, + {"SetWindVelocity", + [&]() { return env.world->SetWindVelocity(1.0f, 2.0f, 3.0f, &bool_out); }, + "/Sim/SceneUnit/SetWindVelocity", + [](const json& p) { + return p["v_x"] == 1.0f && p["v_y"] == 2.0f && p["v_z"] == 3.0f; + }}, + {"SetObjectPose", + [&]() { + return env.world->SetObjectPose("Cube", {{"pose", true}}, true, + &bool_out); + }, + "/Sim/SceneUnit/SetObjectPose", + [](const json& p) { + return p["object_name"] == "Cube" && p["teleport"] == true; + }}, + {"SetObjectScale", + [&]() { + return env.world->SetObjectScale("Cube", {1.0f, 2.0f, 3.0f}, + &bool_out); + }, + "/Sim/SceneUnit/SetObjectScale", + [](const json& p) { + return p["object_name"] == "Cube" && p["scale"].size() == 3; + }}, + {"DestroyObject", + [&]() { return env.world->DestroyObject("Cube", &bool_out); }, + "/Sim/SceneUnit/DestroyObject", + [](const json& p) { return p["object_name"] == "Cube"; }}, + {"DestroyAllSpawnedObjects", + [&]() { return env.world->DestroyAllSpawnedObjects(&bool_out); }, + "/Sim/SceneUnit/DestroyAllSpawnedObjects", + [](const json&) { return true; }}, + {"EnableWeatherVisualEffects", + [&]() { return env.world->EnableWeatherVisualEffects(&bool_out); }, + "/Sim/SceneUnit/SimSetWeatherVisualEffectsStatus", + [](const json& p) { return p["status"] == true; }}, + {"DisableWeatherVisualEffects", + [&]() { return env.world->DisableWeatherVisualEffects(&bool_out); }, + "/Sim/SceneUnit/SimSetWeatherVisualEffectsStatus", + [](const json& p) { return p["status"] == false; }}, + {"ResetWeatherEffects", + [&]() { return env.world->ResetWeatherEffects(&bool_out); }, + "/Sim/SceneUnit/ResetWeatherEffects", + [](const json&) { return true; }}, + {"SetWeatherVisualEffectsParam", + [&]() { + return env.world->SetWeatherVisualEffectsParam(2, 0.75f, &bool_out); + }, + "/Sim/SceneUnit/SetWeatherVisualEffectsParameter", + [](const json& p) { return p["param"] == 2 && p["value"] == 0.75f; }}, + {"SetObjectMaterial", + [&]() { + return env.world->SetObjectMaterial("Cube", "/Game/Mat", &bool_out); + }, + "/Sim/SceneUnit/SetObjectMaterial", + [](const json& p) { + return p["object_name"] == "Cube" && + p["material_asset_path"] == "/Game/Mat"; + }}, + {"SetObjectTextureFromUrl", + [&]() { + return env.world->SetObjectTextureFromUrl("Cube", "http://tex", + &bool_out); + }, + "/Sim/SceneUnit/SetObjectTextureFromUrl", + [](const json& p) { return p["url"] == "http://tex"; }}, + {"SetObjectTextureFromFile", + [&]() { + return env.world->SetObjectTextureFromFile("Cube", "/tmp/t.png", + &bool_out); + }, + "/Sim/SceneUnit/SetObjectTextureFromFile", + [](const json& p) { return p["texture_file_path"] == "/tmp/t.png"; }}, + {"SetObjectTextureFromPackagedAsset", + [&]() { + return env.world->SetObjectTextureFromPackagedAsset( + "Cube", "/Game/Tex", &bool_out); + }, + "/Sim/SceneUnit/SetObjectTextureFromPackagedAsset", + [](const json& p) { return p["texture_asset_path"] == "/Game/Tex"; }}, + {"SwapObjectTexture", + [&]() { return env.world->SwapObjectTexture("tag", 3, &bool_out); }, + "/Sim/SceneUnit/SwapObjectTexture", + [](const json& p) { return p["tag"] == "tag" && p["tex_id"] == 3; }}, + {"SetLightObjectIntensity", + [&]() { + return env.world->SetLightObjectIntensity("Light", 50.0f, &bool_out); + }, + "/Sim/SceneUnit/SetLightObjectIntensity", + [](const json& p) { return p["new_intensity"] == 50.0f; }}, + {"SetLightObjectColor", + [&]() { + return env.world->SetLightObjectColor("Light", {1.0f, 0.5f, 0.0f}, + &bool_out); + }, + "/Sim/SceneUnit/SetLightObjectColor", + [](const json& p) { return p["color_rgb"].size() == 3; }}, + {"SetLightObjectRadius", + [&]() { return env.world->SetLightObjectRadius("Light", 2.0f, &bool_out); }, + "/Sim/SceneUnit/SetLightObjectRadius", + [](const json& p) { return p["new_radius"] == 2.0f; }}, + {"SetTimeOfDay", + [&]() { + return env.world->SetTimeOfDay(true, "2026-01-01 12:00:00", false, + 1.0f, 60.0f, true, &bool_out); + }, + "/Sim/SceneUnit/SetTimeOfDay", + [](const json& p) { + return p["status"] == true && p["move_sun"] == true && + p["datetime"] == "2026-01-01 12:00:00"; + }}, + {"SetSunPositionFromDateTime", + [&]() { + return env.world->SetSunPositionFromDateTime("2026-01-01", false, + &bool_out); + }, + "/Sim/SceneUnit/SetSunPositionFromDateTime", + [](const json& p) { return p["date_time"] == "2026-01-01"; }}, + {"SwitchStreamingView", + [&]() { return env.world->SwitchStreamingView(&bool_out); }, + "/Sim/SceneUnit/SwitchStreamingView", + [](const json&) { return true; }}, + {"SetSegmentationIDByName", + [&]() { + return env.world->SetSegmentationIDByName("Cube", 7, false, true, + &bool_out); + }, + "/Sim/SceneUnit/SetSegmentationIDByName", + [](const json& p) { + return p["mesh_name"] == "Cube" && p["segmentation_id"] == 7 && + p["use_owner_name"] == true; + }}, + {"SetTraceLine", + [&]() { + asc::ColorRGBA color{1.0f, 0.0f, 0.0f, 1.0f}; + return env.world->SetTraceLine(color, 2.0f, &bool_out); + }, + "/Sim/SceneUnit/SetTraceLine", + [](const json& p) { return p["thickness"] == 2.0f; }}, + {"ToggleTrace", + [&]() { return env.world->ToggleTrace(&bool_out); }, + "/Sim/SceneUnit/ToggleTrace", + [](const json&) { return true; }}, + }; + + for (const auto& c : bool_cases) { + push(true); + if (!Expect(c.call() == asc::Status::OK, c.name + " should succeed") || + !ExpectLastRequest(c.method, c.params, c.name)) { + return false; + } + } + + struct JsonCase { + std::string name; + std::function call; + std::string method; + json result; + std::function params; + }; + + const std::vector json_cases = { + {"GetSunlightIntensity", + [&]() { return env.world->GetSunlightIntensity(&float_out); }, + "/Sim/SceneUnit/GetSunLightIntensity", 42.0f, + [](const json&) { return true; }}, + {"GetCloudShadowStrength", + [&]() { return env.world->GetCloudShadowStrength(&float_out); }, + "/Sim/SceneUnit/GetCloudShadowStrength", 0.5f, + [](const json&) { return true; }}, + {"GetWindVelocity", + [&]() { return env.world->GetWindVelocity(&vector_out); }, + "/Sim/SceneUnit/GetWindVelocity", json::array({1.0f, 2.0f, 3.0f}), + [](const json&) { return true; }}, + {"ListActors", + [&]() { return env.world->ListActors(&names_out); }, + "/Sim/SceneUnit/ListActors", json::array({"Drone1"}), + [](const json&) { return true; }}, + {"ListObjects", + [&]() { return env.world->ListObjects(".*", &names_out); }, + "/Sim/SceneUnit/ListObjects", json::array({"Cube"}), + [](const json& p) { return p["name"] == ".*"; }}, + {"ListAssets", + [&]() { return env.world->ListAssets(".*", &names_out); }, + "/Sim/SceneUnit/ListAssets", json::array({"Asset"}), + [](const json& p) { return p["name"] == ".*"; }}, + {"GetObjectPose", + [&]() { return env.world->GetObjectPose("Cube", &json_out); }, + "/Sim/SceneUnit/GetObjectPose", + json{{"translation", {{"x", 0}, {"y", 0}, {"z", 0}}}, + {"rotation", {{"w", 1}, {"x", 0}, {"y", 0}, {"z", 0}}}}, + [](const json& p) { return p["object_name"] == "Cube"; }}, + {"GetObjectPoses", + [&]() { return env.world->GetObjectPoses({"Cube"}, &json_out); }, + "/Sim/SceneUnit/GetObjectPoses", json::array(), + [](const json& p) { return p["object_names"].size() == 1; }}, + {"GetObjectScale", + [&]() { return env.world->GetObjectScale("Cube", &scale_out); }, + "/Sim/SceneUnit/GetObjectScale", json::array({1.0f, 1.0f, 1.0f}), + [](const json& p) { return p["object_name"] == "Cube"; }}, + {"SpawnObject", + [&]() { + return env.world->SpawnObject("Cube", "/Game/Cube", {{"pose", true}}, + {1.0f, 1.0f, 1.0f}, false, &str_out); + }, + "/Sim/SceneUnit/SpawnObject", "Cube_1", + [](const json& p) { + return p["object_name"] == "Cube" && p["asset_path"] == "/Game/Cube"; + }}, + {"SpawnObjectFromFile", + [&]() { + return env.world->SpawnObjectFromFile( + "Mesh", "gltf", {1, 2, 3}, true, {{"pose", true}}, + {1.0f, 1.0f, 1.0f}, false, &str_out); + }, + "/Sim/SceneUnit/spawnObjectFromFile", "Mesh_1", + [](const json& p) { + return p["object_name"] == "Mesh" && p["file_format"] == "gltf" && + p["is_binary"] == true; + }}, + {"SpawnObjectAtGeo", + [&]() { + return env.world->SpawnObjectAtGeo( + "Geo", "/Game/Cube", 1.0f, 2.0f, 3.0f, + {1.0f, 0.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 1.0f}, false, &str_out); + }, + "/Sim/SceneUnit/spawnObjectAtGeo", "Geo_1", + [](const json& p) { + return p["latitude"] == 1.0f && p["longitude"] == 2.0f; + }}, + {"SpawnObjectFromFileAtGeo", + [&]() { + return env.world->SpawnObjectFromFileAtGeo( + "GeoFile", "gltf", {1}, true, 1.0f, 2.0f, 3.0f, + {1.0f, 0.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 1.0f}, false, &str_out); + }, + "/Sim/SceneUnit/spawnObjectFromFileAtGeo", "GeoFile_1", + [](const json& p) { + return p["object_name"] == "GeoFile" && p["byte_array"].size() == 1; + }}, + {"GetWeatherVisualEffectsParam", + [&]() { return env.world->GetWeatherVisualEffectsParam(&json_out); }, + "/Sim/SceneUnit/GetWeatherVisualEffectsParameter", json{{"0", 0.1f}}, + [](const json&) { return true; }}, + {"GetTimeOfDay", + [&]() { return env.world->GetTimeOfDay(&json_out); }, + "/Sim/SceneUnit/GetTimeOfDay", json{{"enabled", true}}, + [](const json&) { return true; }}, + {"GetSegmentationIDByName", + [&]() { + int seg_id = 0; + return env.world->GetSegmentationIDByName("Cube", true, &seg_id); + }, + "/Sim/SceneUnit/GetSegmentationIDByName", 7, + [](const json& p) { + return p["mesh_name"] == "Cube" && p["use_owner_name"] == true; + }}, + {"GetSegmentationIDMap", + [&]() { return env.world->GetSegmentationIDMap(&json_out); }, + "/Sim/SceneUnit/GetSegmentationIDMap", json{{"Cube", 7}}, + [](const json&) { return true; }}, + {"Get3DBoundingBox", + [&]() { return env.world->Get3DBoundingBox("Cube", 0, &json_out); }, + "/Sim/SceneUnit/Get3DBoundingBox", json{{"name", "Cube"}}, + [](const json& p) { + return p["object_name"] == "Cube" && p["box_alignment"] == 0; + }}, + {"GetSurfaceElevationAtPoint", + [&]() { return env.world->GetSurfaceElevationAtPoint(1.0f, 2.0f, &float_out); }, + "/Sim/SceneUnit/GetSurfaceElevationAtPoint", 12.5f, + [](const json& p) { return p["x"] == 1.0f && p["y"] == 2.0f; }}, + }; + + for (const auto& c : json_cases) { + push(c.result); + if (!Expect(c.call() == asc::Status::OK, c.name + " should succeed") || + !ExpectLastRequest(c.method, c.params, c.name)) { + return false; + } + } + + asc::ColorRGBA color{1.0f, 1.0f, 1.0f, 1.0f}; + asc::VecVector3 points = {{0.0f, 0.0f, 0.0f}}; + asc::VecVector3 points_end = {{1.0f, 0.0f, 0.0f}}; + asc::VecPose poses = {asc::Pose()}; + + const std::vector debug_cases = { + {"FlushPersistentMarkers", + [&]() { return env.world->FlushPersistentMarkers(); }, + "/Sim/SceneUnit/debugFlushPersistentMarkers", + [](const json&) { return true; }}, + {"PlotDebugPoints", + [&]() { return env.world->PlotDebugPoints(points, color, 1.0f, 2.0f, false); }, + "/Sim/SceneUnit/debugPlotPoints", + [](const json& p) { return p["size"] == 1.0f; }}, + {"PlotDebugSolidLine", + [&]() { return env.world->PlotDebugSolidLine(points, color, 1.0f, 2.0f, false); }, + "/Sim/SceneUnit/debugPlotSolidLine", + [](const json& p) { return p["thickness"] == 1.0f; }}, + {"PlotDebugDashedLine", + [&]() { + asc::VecVector3 even_points = {{0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}}; + return env.world->PlotDebugDashedLine(even_points, color, 1.0f, 2.0f, + false); + }, + "/Sim/SceneUnit/debugPlotDashedLine", + [](const json& p) { return p["points"].size() == 2; }}, + {"PlotDebugArrows", + [&]() { + return env.world->PlotDebugArrows(points, points_end, color, 1.0f, 0.5f, + 2.0f, false); + }, + "/Sim/SceneUnit/debugPlotArrows", + [](const json& p) { return p["arrow_size"] == 0.5f; }}, + {"PlotDebugStrings", + [&]() { return env.world->PlotDebugStrings({"hi"}, points, 1.0f, color, 2.0f); }, + "/Sim/SceneUnit/debugPlotStrings", + [](const json& p) { return p["strings"].size() == 1; }}, + {"PlotDebugTransforms", + [&]() { return env.world->PlotDebugTransforms(poses, 1.0f, 0.5f, 2.0f, false); }, + "/Sim/SceneUnit/debugPlotTransforms", + [](const json& p) { return p["poses"].size() == 1; }}, + {"PlotDebugTransformsWithNames", + [&]() { + return env.world->PlotDebugTransformsWithNames(poses, {"pose"}, 1.0f, + 0.5f, 1.0f, color, 2.0f); + }, + "/Sim/SceneUnit/debugPlotTransformsWithNames", + [](const json& p) { return p["names"].size() == 1; }}, + {"ImportGeoTrajectory", + [&]() { + return env.world->ImportGeoTrajectory("geo", {0.0f}, {1.0f}, {2.0f}, + {3.0f}); + }, + "/Sim/SceneUnit/ImportGeoTrajectory", + [](const json& p) { return p["traj_name"] == "geo"; }}, + }; + + for (const auto& c : debug_cases) { + push(true); + if (!Expect(c.call() == asc::Status::OK, c.name + " should succeed") || + !ExpectLastRequest(c.method, c.params, c.name)) { + return false; + } + } + + push(true); + if (!Expect(env.world->ImportNEDTrajectory("ned", {0.0f}, {1.0f}, {2.0f}, + {3.0f}) == asc::Status::OK, + "World::ImportNEDTrajectory should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/ImportNEDTrajectory", + [](const json& p) { return p["traj_name"] == "ned"; }, + "World::ImportNEDTrajectory")) { + return false; + } + + push(json::array({true, false})); + asc::BoolArray voxel_grid; + if (!Expect(env.world->CreateVoxelGrid(asc::Pose(), 2, 2, 2, 1.0f, + &voxel_grid, {"Drone1"}, false) == + asc::Status::OK, + "World::CreateVoxelGrid should succeed") || + !Expect(voxel_grid.Cf() == 2, "World::CreateVoxelGrid should parse bool array") || + !ExpectLastRequest("/Sim/SceneUnit/createVoxelGrid", + [](const json& p) { + return p["x_size"] == 2 && p["y_size"] == 2 && + p["z_size"] == 2 && p["res"] == 1.0f && + p["actors_to_ignore"].size() == 1; + }, + "World::CreateVoxelGrid")) { + return false; + } + + env.client->Disconnect(); + return true; +} + +bool TestDroneAPIsUseExpectedServiceMethods() { + TestEnvironment env; + if (!CreateDrone(&env)) return false; + + int id = 0; + auto push = [&](const json& result) { + fake_nngi::PushServiceResponse(SuccessResponse(id++, result)); + }; + + bool bool_out = false; + float float_out = 0.0f; + json json_out; + asc::ReadyState ready_state; + asc::LandedState landed_state = asc::LandedState::Flying; + asc::Transform transform_out; + asc::GeoPosition geo_out; + + struct DroneCase { + std::string name; + std::function call; + std::string method; + std::function params; + }; + + const std::vector cases = { + {"EnableAPIControl", + [&]() { return env.drone->EnableAPIControl(&bool_out); }, + "/Sim/SceneUnit/robots/Drone1/EnableApiControl", + [](const json&) { return true; }}, + {"DisableAPIControl", + [&]() { return env.drone->DisableAPIControl(&bool_out); }, + "/Sim/SceneUnit/robots/Drone1/DisableApiControl", + [](const json&) { return true; }}, + {"IsAPIControlEnabled", + [&]() { return env.drone->IsAPIControlEnabled(&bool_out); }, + "/Sim/SceneUnit/robots/Drone1/IsApiControlEnabled", + [](const json&) { return true; }}, + {"Arm", + [&]() { return env.drone->Arm(&bool_out); }, + "/Sim/SceneUnit/robots/Drone1/Arm", + [](const json&) { return true; }}, + {"Disarm", + [&]() { return env.drone->Disarm(&bool_out); }, + "/Sim/SceneUnit/robots/Drone1/Disarm", + [](const json&) { return true; }}, + {"CanArm", + [&]() { return env.drone->CanArm(&bool_out); }, + "/Sim/SceneUnit/robots/Drone1/CanArm", + [](const json&) { return true; }}, + {"SetGroundTruthKinematics", + [&]() { + return env.drone->SetGroundTruthKinematics({{"linear", true}}, + &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/SetGroundTruthKinematics", + [](const json& p) { return p["kinematics"]["linear"] == true; }}, + {"SetCameraPose", + [&]() { + return env.drone->SetCameraPose("Camera1", {{"pose", true}}, true, + &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/SetPose", + [](const json& p) { + return p["pose"]["pose"] == true && p["wait_for_pose_update"] == true; + }}, + {"ResetCameraPose", + [&]() { return env.drone->ResetCameraPose("Camera1", false, &bool_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/ResetCameraPose", + [](const json& p) { return p["wait_for_pose_update"] == false; }}, + {"SetFocalLength", + [&]() { return env.drone->SetFocalLength("Camera1", 0, 35.0f, &bool_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/SetFocalLength", + [](const json& p) { + return p["image_type_id"] == 0 && p["focal_length"] == 35.0f; + }}, + {"SetDepthOfFieldTransitionThreshold", + [&]() { + return env.drone->SetDepthOfFieldTransitionThreshold("Camera1", 0, 4.0f, + &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/SetDepthOfFieldTransitionRegion", + [](const json& p) { return p["transition_threshold"] == 4.0f; }}, + {"SetDepthOfFieldFocalRegion", + [&]() { + return env.drone->SetDepthOfFieldFocalRegion("Camera1", 0, 10.0f, + &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/SetDepthOfFieldFocalRegion", + [](const json& p) { return p["focal_length"] == 10.0f; }}, + {"SetChromaticAberrationIntensity", + [&]() { + return env.drone->SetChromaticAberrationIntensity("Camera1", 0, 0.25f, + &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/SetChromaticAberrationIntensity", + [](const json& p) { return p["intensity"] == 0.25f; }}, + {"SetFieldOfView", + [&]() { return env.drone->SetFieldOfView("Camera1", 0, 90.0f, &bool_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/SetFieldOfView", + [](const json& p) { return p["field_of_view"] == 90.0f; }}, + {"SetControlSignals", + [&]() { + return env.drone->SetControlSignals({{"0", 0.5f}}, &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/SetControlSignals", + [](const json& p) { return p["control_signal_map"]["0"] == 0.5f; }}, + {"UpdateActuatorFaultState", + [&]() { + return env.drone->UpdateActuatorFaultState("0", true, &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/actuators/0/ToggleFault", + [](const json& p) { return p["enable"] == true; }}, + {"SetExternalForce", + [&]() { return env.drone->SetExternalForce({1.0f, 2.0f, 3.0f}, &bool_out); }, + "/Sim/SceneUnit/robots/Drone1/SetExternalForce", + [](const json& p) { return p["ext_force"].size() == 3; }}, + {"SetBatteryRemaining", + [&]() { return env.drone->SetBatteryRemaining(0.5f, &bool_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Battery/SetBatteryRemaining", + [](const json& p) { return p["desired_battery_remaining"] == 0.5f; }}, + {"SetBatteryDrainRate", + [&]() { return env.drone->SetBatteryDrainRate(0.25f, &bool_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Battery/SetBatteryDrainRate", + [](const json& p) { return p["desired_drain_rate"] == 0.25f; }}, + {"SetBatteryHealthStatus", + [&]() { return env.drone->SetBatteryHealthStatus(true, &bool_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Battery/SetBatteryHealthStatus", + [](const json& p) { return p["battery_health_indicator"] == true; }}, + {"CameraLookAtObject", + [&]() { + return env.drone->CameraLookAtObject("Camera1", "Cube", true, + &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/LookAtObject", + [](const json& p) { + return p["object_name"] == "Cube" && p["wait_for_pose_update"] == true; + }}, + {"CameraDrawFrustum", + [&]() { + return env.drone->CameraDrawFrustum("Camera1", true, + asc::Drone::ImageType::Scene, + &bool_out); + }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/DrawFrustum", + [](const json& p) { return p["to_enable"] == true && p["image_type"] == 0; }}, + }; + + for (const auto& c : cases) { + push(true); + if (!Expect(c.call() == asc::Status::OK, c.name + " should succeed") || + !ExpectLastRequest(c.method, c.params, c.name)) { + return false; + } + } + + struct AsyncCase { + std::string name; + std::function call; + std::string method; + std::function params; + }; + + const std::vector async_cases = { + {"TakeoffAsync", + [&]() { return env.drone->TakeoffAsync(5.0f); }, + "/Sim/SceneUnit/robots/Drone1/Takeoff", + [](const json& p) { return p["timeout_sec"] == 5.0f; }}, + {"LandAsync", + [&]() { return env.drone->LandAsync(6.0f); }, + "/Sim/SceneUnit/robots/Drone1/Land", + [](const json& p) { return p["timeout_sec"] == 6.0f; }}, + {"GoHomeAsync", + [&]() { return env.drone->GoHomeAsync(7.0f, 1.5f); }, + "/Sim/SceneUnit/robots/Drone1/GoHome", + [](const json& p) { + return p["timeout_sec"] == 7.0f && p["velocity"] == 1.5f; + }}, + {"HoverAsync", + [&]() { return env.drone->HoverAsync(); }, + "/Sim/SceneUnit/robots/Drone1/Hover", + [](const json&) { return true; }}, + {"RequestControlAsync", + [&]() { return env.drone->RequestControlAsync(); }, + "/Sim/SceneUnit/robots/Drone1/RequestControl", + [](const json&) { return true; }}, + {"SetMissionModeAsync", + [&]() { return env.drone->SetMissionModeAsync(); }, + "/Sim/SceneUnit/robots/Drone1/SetMissionMode", + [](const json&) { return true; }}, + {"SetVTOLModeAsync", + [&]() { return env.drone->SetVTOLModeAsync(asc::Drone::VTOLMode::FixedWing); }, + "/Sim/SceneUnit/robots/Drone1/SetVTOLMode", + [](const json& p) { return p["vtol_mode"] == 1; }}, + {"MoveByHeadingAsync", + [&]() { + return env.drone->MoveByHeadingAsync(1.0f, 2.0f, -0.5f, 3.0f, 0.1f, + 0.2f, 4.0f); + }, + "/Sim/SceneUnit/robots/Drone1/MoveByHeading", + [](const json& p) { + return p["heading"] == 1.0f && p["speed"] == 2.0f && + p["vz"] == -0.5f; + }}, + {"MoveByVelocityAsync", + [&]() { + return env.drone->MoveByVelocityAsync(1.0f, 2.0f, 3.0f, 4.0f); + }, + "/Sim/SceneUnit/robots/Drone1/MoveByVelocity", + [](const json& p) { return p["vx"] == 1.0f && p["duration"] == 4.0f; }}, + {"MoveByVelocityZAsync", + [&]() { + return env.drone->MoveByVelocityZAsync(1.0f, 2.0f, -3.0f, 4.0f); + }, + "/Sim/SceneUnit/robots/Drone1/MoveByVelocityZ", + [](const json& p) { return p["z"] == -3.0f && p["duration"] == 4.0f; }}, + {"MoveByVelocityBodyFrameAsync", + [&]() { + return env.drone->MoveByVelocityBodyFrameAsync(1.0f, 2.0f, 3.0f, + 4.0f); + }, + "/Sim/SceneUnit/robots/Drone1/MoveByVelocityBodyFrame", + [](const json& p) { return p["vx"] == 1.0f && p["vy"] == 2.0f; }}, + {"MoveByVelocityBodyFrameZAsync", + [&]() { + return env.drone->MoveByVelocityBodyFrameZAsync(1.0f, 2.0f, -3.0f, + 4.0f); + }, + "/Sim/SceneUnit/robots/Drone1/MoveByVelocityBodyFrameZ", + [](const json& p) { return p["z"] == -3.0f && p["duration"] == 4.0f; }}, + {"MoveToPositionAsync", + [&]() { + return env.drone->MoveToPositionAsync(1.0f, 2.0f, -3.0f, 4.0f, + 5.0f); + }, + "/Sim/SceneUnit/robots/Drone1/MoveToPosition", + [](const json& p) { return p["x"] == 1.0f && p["z"] == -3.0f; }}, + {"MoveOnPathAsync", + [&]() { + return env.drone->MoveOnPathAsync({{1.0f, 2.0f, -3.0f}}, 4.0f, 5.0f); + }, + "/Sim/SceneUnit/robots/Drone1/MoveOnPath", + [](const json& p) { return p["path"].size() == 1 && p["velocity"] == 4.0f; }}, + {"RotateToYawAsync", + [&]() { return env.drone->RotateToYawAsync(90.0f, 5.0f, 0.1f, 0.2f); }, + "/Sim/SceneUnit/robots/Drone1/RotateToYaw", + [](const json& p) { return p["yaw"] == 90.0f && p["timeout_sec"] == 5.0f; }}, + {"RotateByYawRateAsync", + [&]() { return env.drone->RotateByYawRateAsync(0.5f, 2.0f); }, + "/Sim/SceneUnit/robots/Drone1/RotateByYawRate", + [](const json& p) { return p["yaw_rate"] == 0.5f && p["duration"] == 2.0f; }}, + }; + + for (const auto& c : async_cases) { + push(true); + auto result = c.call(); + if (!Expect(result.Wait() == asc::Status::OK, + c.name + " should complete successfully") || + !ExpectLastRequest(c.method, c.params, c.name)) { + return false; + } + } + + push({{"imu", true}}); + if (!Expect(env.drone->GetIMUData("IMU1", &json_out) == asc::Status::OK, + "Drone::GetIMUData should succeed") || + !ExpectLastRequest( + "/Sim/SceneUnit/robots/Drone1/sensors/IMU1/imu_kinematics", + "Drone::GetIMUData")) { + return false; + } + + struct JsonDroneCase { + std::string name; + std::function call; + std::string method; + json result; + std::function params; + }; + + const std::vector json_cases = { + {"GetGPSData", + [&]() { return env.drone->GetGPSData("GPS", &json_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/GPS/gps", json{{"gps", true}}, + [](const json&) { return true; }}, + {"GetAirspeedData", + [&]() { return env.drone->GetAirspeedData("Airspeed", &json_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Airspeed/airspeed", + json{{"airspeed", true}}, [](const json&) { return true; }}, + {"GetBatteryState", + [&]() { return env.drone->GetBatteryState(&json_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Battery/GetBatteryStatus", + json{{"battery", true}}, [](const json&) { return true; }}, + {"GetBatteryDrainRate", + [&]() { return env.drone->GetBatteryDrainRate(&float_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Battery/GetBatteryDrainRate", 0.1f, + [](const json&) { return true; }}, + {"GetBarometerData", + [&]() { return env.drone->GetBarometerData("Barometer", &json_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Barometer/barometer", + json{{"barometer", true}}, [](const json&) { return true; }}, + {"GetMagnetometerData", + [&]() { return env.drone->GetMagnetometerData("Magnetometer", &json_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Magnetometer/magnetometer", + json{{"magnetometer", true}}, [](const json&) { return true; }}, + {"GetGroundTruthKinematics", + [&]() { return env.drone->GetGroundTruthKinematics(&json_out); }, + "/Sim/SceneUnit/robots/Drone1/GetGroundTruthKinematics", + json{{"kinematics", true}}, [](const json&) { return true; }}, + {"GetEstimatedKinematics", + [&]() { return env.drone->GetEstimatedKinematics(&json_out); }, + "/Sim/SceneUnit/robots/Drone1/GetEstimatedKinematics", + json{{"kinematics", true}}, [](const json&) { return true; }}, + {"GetImages", + [&]() { return env.drone->GetImages("Camera1", {0, 1}, &json_out); }, + "/Sim/SceneUnit/robots/Drone1/sensors/Camera1/GetImages", + json::array({json{{"image", true}}}), + [](const json& p) { return p["image_type_ids"].size() == 2; }}, + }; + + for (const auto& c : json_cases) { + push(c.result); + if (!Expect(c.call() == asc::Status::OK, c.name + " should succeed") || + !ExpectLastRequest(c.method, c.params, c.name)) { + return false; + } + } + + push(json{{"ready_val", true}, {"ready_message", "ready"}}); + if (!Expect(env.drone->GetReadyState(&ready_state) == asc::Status::OK, + "Drone::GetReadyState should succeed") || + !Expect(ready_state.is_ready, "Drone::GetReadyState should parse result") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Drone1/GetReadyState", + "Drone::GetReadyState")) { + return false; + } + + push(0); + if (!Expect(env.drone->GetLandedState(&landed_state) == asc::Status::OK, + "Drone::GetLandedState should succeed") || + !Expect(landed_state == asc::LandedState::Landed, + "Drone::GetLandedState should parse result") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Drone1/GetLandedState", + "Drone::GetLandedState")) { + return false; + } + + push(TransformResultJson()); + if (!Expect(env.drone->GetGroundTruthPose(&transform_out) == asc::Status::OK, + "Drone::GetGroundTruthPose should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Drone1/GetGroundTruthPose", + "Drone::GetGroundTruthPose")) { + return false; + } + + push(GeoPositionResultJson()); + if (!Expect(env.drone->GetGroundTruthGeoLocation(&geo_out) == asc::Status::OK, + "Drone::GetGroundTruthGeoLocation should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Drone1/GetGroundTruthGeoLocation", + "Drone::GetGroundTruthGeoLocation")) { + return false; + } + + push(GeoPositionResultJson()); + if (!Expect(env.drone->GetEstimatedGeoLocation(&geo_out) == asc::Status::OK, + "Drone::GetEstimatedGeoLocation should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Drone1/GetEstimatedGeoLocation", + "Drone::GetEstimatedGeoLocation")) { + return false; + } + + push(true); + if (!Expect(env.drone->SetPose(asc::Transform(), true) == asc::Status::OK, + "Drone::SetPose should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Drone1/SetPose", + [](const json& p) { + return p["reset_kinematics"] == true && + p.contains("pose"); + }, + "Drone::SetPose")) { + return false; + } + + push(true); + if (!Expect(env.drone->CancelLastTask(&bool_out) == asc::Status::OK, + "Drone::CancelLastTask should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Drone1/CancelLastTask", + "Drone::CancelLastTask")) { + return false; + } + + env.client->Disconnect(); + return true; +} + +bool TestRoverAPIsUseExpectedServiceMethods() { + TestEnvironment env; + if (!CreateRover(&env)) return false; + + int id = 0; + auto push = [&](const json& result) { + fake_nngi::PushServiceResponse(SuccessResponse(id++, result)); + }; + + bool bool_out = false; + json json_out; + + struct RoverCase { + std::string name; + std::function call; + std::string method; + std::function params; + }; + + const std::vector cases = { + {"Arm", [&]() { return env.rover->Arm(&bool_out); }, + "/Sim/SceneUnit/robots/Rover1/Arm", [](const json&) { return true; }}, + {"CanArm", [&]() { return env.rover->CanArm(&bool_out); }, + "/Sim/SceneUnit/robots/Rover1/CanArm", [](const json&) { return true; }}, + {"DisableAPIControl", + [&]() { return env.rover->DisableAPIControl(&bool_out); }, + "/Sim/SceneUnit/robots/Rover1/DisableApiControl", + [](const json&) { return true; }}, + {"Disarm", [&]() { return env.rover->Disarm(&bool_out); }, + "/Sim/SceneUnit/robots/Rover1/Disarm", [](const json&) { return true; }}, + {"EnableAPIControl", + [&]() { return env.rover->EnableAPIControl(&bool_out); }, + "/Sim/SceneUnit/robots/Rover1/EnableApiControl", + [](const json&) { return true; }}, + {"SetPose", + [&]() { return env.rover->SetPose(asc::Transform(), true); }, + "/Sim/SceneUnit/robots/Rover1/SetPose", + [](const json& p) { return p["reset_kinematics"] == true; }}, + {"SetRoverControls", + [&]() { return env.rover->SetRoverControls(0.5f, 0.1f, 0.0f); }, + "/Sim/SceneUnit/robots/Rover1/SetRoverControls", + [](const json& p) { + return p["engine"] == 0.5f && p["steering_angle"] == 0.1f; + }}, + }; + + for (const auto& c : cases) { + push(true); + if (!Expect(c.call() == asc::Status::OK, c.name + " should succeed") || + !ExpectLastRequest(c.method, c.params, "Rover::" + c.name)) { + return false; + } + } + + push(json{{"kinematics", true}}); + if (!Expect(env.rover->GetGroundTruthKinematics(&json_out) == asc::Status::OK, + "Rover::GetGroundTruthKinematics should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Rover1/GetGroundTruthKinematics", + "Rover::GetGroundTruthKinematics")) { + return false; + } + + push(true); + if (!Expect(env.rover->CancelLastTask(&bool_out) == asc::Status::OK, + "Rover::CancelLastTask should succeed") || + !ExpectLastRequest("/Sim/SceneUnit/robots/Rover1/CancelLastTask", + "Rover::CancelLastTask")) { + return false; + } + + struct AsyncCase { + std::string name; + std::function call; + std::string method; + std::function params; + }; + + const std::vector async_cases = { + {"MoveToPositionAsync", + [&]() { return env.rover->MoveToPositionAsync(1.0f, 2.0f, 3.0f, 4.0f); }, + "/Sim/SceneUnit/robots/Rover1/MoveToPosition", + [](const json& p) { + return p["x"] == 1.0f && p["y"] == 2.0f && + p["velocity"] == 3.0f; + }}, + {"MoveByHeadingAsync", + [&]() { return env.rover->MoveByHeadingAsync(1.0f, 2.0f, 3.0f); }, + "/Sim/SceneUnit/robots/Rover1/MoveByHeading", + [](const json& p) { + return p["heading"] == 1.0f && p["speed"] == 2.0f && + p["duration"] == 3.0f; + }}, + }; + + for (const auto& c : async_cases) { + push(true); + auto result = c.call(); + if (!Expect(result.Wait() == asc::Status::OK, + "Rover::" + c.name + " should complete successfully") || + !ExpectLastRequest(c.method, c.params, "Rover::" + c.name)) { + return false; + } + } + + env.client->Disconnect(); + return true; +} + +bool TestEnvActorAPIsUseExpectedServiceMethods() { + TestEnvironment env; + if (!CreateEnvActor(&env)) return false; + + int id = 0; + auto push = [&](const json& result) { + fake_nngi::PushServiceResponse(SuccessResponse(id++, result)); + }; + + struct EnvActorCase { + std::string name; + std::function call; + std::string method; + std::function params; + }; + + const std::vector cases = { + {"SetTrajectory", + [&]() { + return env.env_actor->SetTrajectory("traj", true, 1.0f, 2.0f, 3.0f, + 4.0f, 0.1f, 0.2f, 0.3f); + }, + "/Sim/SceneUnit/SetEnvActorTrajectory", + [](const json& p) { + return p["env_actor_name"] == "Actor1" && p["traj_name"] == "traj" && + p["to_loop"] == true; + }}, + {"SetLinkRotationAngle", + [&]() { return env.env_actor->SetLinkRotationAngle("link", 45.0f); }, + "/Sim/SceneUnit/SetEnvActorLinkRotAngle", + [](const json& p) { + return p["env_actor_name"] == "Actor1" && p["link_name"] == "link" && + p["angle_deg"] == 45.0f; + }}, + {"SetLinkRotationRate", + [&]() { return env.env_actor->SetLinkRotationRate("link", 5.0f); }, + "/Sim/SceneUnit/SetEnvActorLinkRotRate", + [](const json& p) { + return p["env_actor_name"] == "Actor1" && p["link_name"] == "link" && + p["rotation_deg_per_sec"] == 5.0f; + }}, + }; + + for (const auto& c : cases) { + push(true); + if (!Expect(c.call() == asc::Status::OK, c.name + " should succeed") || + !ExpectLastRequest(c.method, c.params, "EnvActor::" + c.name)) { + return false; + } + } + + env.client->Disconnect(); + return true; +} + +bool TestStaticSensorActorAPIsUseExpectedServiceMethods() { + TestEnvironment env; + if (!CreateStaticSensorActor(&env)) return false; + + fake_nngi::PushServiceResponse( + SuccessResponse(0, json::array({json{{"image", true}}}))); + json json_out; + if (!Expect(env.static_sensor_actor->GetImages("Camera1", {0, 1}, + &json_out) == + asc::Status::OK, + "StaticSensorActor::GetImages should succeed") || + !ExpectLastRequest( + "/Sim/SceneUnit/robots/Static1/sensors/Camera1/GetImages", + [](const json& p) { return p["image_type_ids"].size() == 2; }, + "StaticSensorActor::GetImages")) { + return false; + } + + env.client->Disconnect(); + return true; +} + +} // namespace + +int main() { + int failed = 0; + + failed += TestRequestBeforeConnectFails() ? 0 : 1; + failed += TestConnectAndRequest() ? 0 : 1; + failed += TestGetBuildCommitHashUsesExpectedServiceMethod() ? 0 : 1; + failed += TestRejectedResponseReturnsRejectedByServer() ? 0 : 1; + failed += TestClientPublishSendsTopicMessage() ? 0 : 1; + failed += TestClientTopicAndLifecycleAPIsUseExpectedMessages() ? 0 : 1; + failed += TestWorldAPIsUseExpectedServiceMethods() ? 0 : 1; + failed += TestDroneAPIsUseExpectedServiceMethods() ? 0 : 1; + failed += TestRoverAPIsUseExpectedServiceMethods() ? 0 : 1; + failed += TestEnvActorAPIsUseExpectedServiceMethods() ? 0 : 1; + failed += TestStaticSensorActorAPIsUseExpectedServiceMethods() ? 0 : 1; + + if (failed == 0) { + std::cout << "[OK] C++ client unit tests passed" << std::endl; + return 0; + } + + std::cerr << "[FAIL] " << failed << " C++ client unit test(s) failed" << std::endl; + return 1; +} diff --git a/client/cpp/tests/FakeNNGI.cpp b/client/cpp/tests/FakeNNGI.cpp new file mode 100644 index 00000000..cdf8ef25 --- /dev/null +++ b/client/cpp/tests/FakeNNGI.cpp @@ -0,0 +1,170 @@ +#include "FakeNNGI.h" + +#include + +#include +#include +#include + +namespace projectairsim_cpp_tests { +namespace fake_nngi { +namespace { + +std::mutex g_mutex; +std::deque g_service_responses; +std::deque g_topic_messages; +std::vector g_sent_service_messages; +std::vector g_sent_topic_messages; +std::vector g_dialed_urls; +uint32_t g_next_socket_id = 1; +constexpr uint32_t kServiceSocketId = 1; +constexpr uint32_t kTopicSocketId = 2; + +} // namespace + +std::mutex& Mutex() { return g_mutex; } +std::deque& ServiceResponses() { return g_service_responses; } +std::deque& TopicMessages() { return g_topic_messages; } +std::vector& SentServiceMessagesStore() { + return g_sent_service_messages; +} +std::vector& SentTopicMessagesStore() { + return g_sent_topic_messages; +} +std::vector& DialedUrlsStore() { return g_dialed_urls; } +uint32_t& NextSocketId() { return g_next_socket_id; } +uint32_t ServiceSocketId() { return kServiceSocketId; } +uint32_t TopicSocketId() { return kTopicSocketId; } + +void Reset() { + std::lock_guard lock(g_mutex); + g_service_responses.clear(); + g_topic_messages.clear(); + g_sent_service_messages.clear(); + g_sent_topic_messages.clear(); + g_dialed_urls.clear(); + g_next_socket_id = 1; +} + +void PushServiceResponse(const std::string& response) { + std::lock_guard lock(g_mutex); + g_service_responses.push_back(response); +} + +void PushTopicMessage(const std::string& message) { + std::lock_guard lock(g_mutex); + g_topic_messages.push_back(message); +} + +std::vector SentServiceMessages() { + std::lock_guard lock(g_mutex); + return g_sent_service_messages; +} + +std::vector SentTopicMessages() { + std::lock_guard lock(g_mutex); + return g_sent_topic_messages; +} + +std::vector DialedUrls() { + std::lock_guard lock(g_mutex); + return g_dialed_urls; +} + +} // namespace fake_nngi +} // namespace projectairsim_cpp_tests + +namespace nngi { + +int nng_close(nng_socket) { return 0; } + +int nng_dial(nng_socket, const char* addr, nng_dialer*, int) { + std::lock_guard lock(projectairsim_cpp_tests::fake_nngi::Mutex()); + projectairsim_cpp_tests::fake_nngi::DialedUrlsStore().emplace_back(addr); + return 0; +} + +void nng_free(void* buf, std::size_t) { delete[] static_cast(buf); } + +int nng_pair0_open(nng_socket* socket) { + std::lock_guard lock(projectairsim_cpp_tests::fake_nngi::Mutex()); + socket->id = projectairsim_cpp_tests::fake_nngi::NextSocketId()++; + return 0; +} + +int nng_req0_open(nng_socket* socket) { + std::lock_guard lock(projectairsim_cpp_tests::fake_nngi::Mutex()); + socket->id = projectairsim_cpp_tests::fake_nngi::NextSocketId()++; + return 0; +} + +int nng_recv(nng_socket socket, void* buf, std::size_t* size, int flags) { + if ((flags & NNG_FLAG_ALLOC) == 0) return NNG_EINVAL; + + std::lock_guard lock(projectairsim_cpp_tests::fake_nngi::Mutex()); + if (socket.id == projectairsim_cpp_tests::fake_nngi::TopicSocketId()) { + if (projectairsim_cpp_tests::fake_nngi::TopicMessages().empty()) { + return NNG_ETIMEDOUT; + } + + const std::string response = + projectairsim_cpp_tests::fake_nngi::TopicMessages().front(); + projectairsim_cpp_tests::fake_nngi::TopicMessages().pop_front(); + + auto* bytes = new uint8_t[response.size()]; + std::memcpy(bytes, response.data(), response.size()); + *static_cast(buf) = bytes; + *size = response.size(); + return 0; + } + + if (socket.id != projectairsim_cpp_tests::fake_nngi::ServiceSocketId()) { + return NNG_ETIMEDOUT; + } + if (projectairsim_cpp_tests::fake_nngi::ServiceResponses().empty()) { + return NNG_ECANCELED; + } + + const std::string response = + projectairsim_cpp_tests::fake_nngi::ServiceResponses().front(); + projectairsim_cpp_tests::fake_nngi::ServiceResponses().pop_front(); + + auto* bytes = new uint8_t[response.size()]; + std::memcpy(bytes, response.data(), response.size()); + *static_cast(buf) = bytes; + *size = response.size(); + return 0; +} + +int nng_send(nng_socket socket, void* buf, std::size_t len, int) { + std::lock_guard lock(projectairsim_cpp_tests::fake_nngi::Mutex()); + if (socket.id == projectairsim_cpp_tests::fake_nngi::ServiceSocketId()) { + projectairsim_cpp_tests::fake_nngi::SentServiceMessagesStore().emplace_back( + static_cast(buf), len); + } else if (socket.id == projectairsim_cpp_tests::fake_nngi::TopicSocketId()) { + projectairsim_cpp_tests::fake_nngi::SentTopicMessagesStore().emplace_back( + static_cast(buf), len); + } + return 0; +} + +int nng_socket_set_ms(nng_socket, const char*, nng_duration) { return 0; } + +const char* nng_strerror(int error) { + switch (error) { + case NNG_ETIMEDOUT: + return "timed out"; + case NNG_ECLOSED: + return "closed"; + case NNG_ECANCELED: + return "canceled"; + case NNG_EINVAL: + return "invalid"; + default: + return "fake nng error"; + } +} + +const char* nng_version(void) { return "fake-nng"; } + +} // namespace nngi diff --git a/client/cpp/tests/FakeNNGI.h b/client/cpp/tests/FakeNNGI.h new file mode 100644 index 00000000..3039f534 --- /dev/null +++ b/client/cpp/tests/FakeNNGI.h @@ -0,0 +1,19 @@ +#pragma once + +#include +#include +#include +#include + +namespace projectairsim_cpp_tests { +namespace fake_nngi { + +void Reset(); +void PushServiceResponse(const std::string& response); +void PushTopicMessage(const std::string& message); +std::vector SentServiceMessages(); +std::vector SentTopicMessages(); +std::vector DialedUrls(); + +} // namespace fake_nngi +} // namespace projectairsim_cpp_tests diff --git a/client/cpp/tests/README.md b/client/cpp/tests/README.md new file mode 100644 index 00000000..46dbbf91 --- /dev/null +++ b/client/cpp/tests/README.md @@ -0,0 +1,67 @@ +# ProjectAirSim C++ Client Tests + +Este directorio tiene tests para el cliente C++. + +## Tipos de tests + +| Test | Target | Usa Unreal/simserver real | Para que sirve | +| --- | --- | --- | --- | +| Mocked unit test | `cpp_client_mocked_unit_tests` | No | Valida el cliente C++ con NNG mockeado (`FakeNNGI`). | +| Integration test | `cpp_client_unit_tests` | Si | Valida que el cliente C++ pueda conectarse a Unreal/ProjectAirSim real. | + +## Compilar + +Desde la raiz del repo: + +```bash +cmake --build client/cpp/build_linux/Debug --target cpp_client_mocked_unit_tests -j16 +cmake --build client/cpp/build_linux/Debug --target cpp_client_unit_tests -j16 +``` + +## Correr test mockeado + +Este test no necesita Unreal corriendo. + +```bash +./client/cpp/build_linux/Debug/cpp_client_mocked_unit_tests +``` + +Resultado esperado: + +```text +[OK] C++ client unit tests passed +``` + +## Correr test contra Unreal real + +Primero levantar Unreal/ProjectAirSim o el simserver. Despues: + +```bash +./client/cpp/build_linux/Debug/cpp_client_unit_tests +``` + +Si Unreal esta corriendo y el server escucha en `127.0.0.1`, deberia pasar. + +Si Unreal no esta corriendo, debe fallar. Eso es esperado: + +```text +[FAIL] Client::Connect(127.0.0.1) failed: ... +[INFO] Start Unreal/ProjectAirSim simserver before running this integration test. +``` + +## Usar otro host + +El integration test acepta el host como primer argumento: + +```bash +./client/cpp/build_linux/Debug/cpp_client_unit_tests 192.168.0.50 +``` + +## Archivos + +| Archivo | Descripcion | +| --- | --- | +| `CppClientUnitTests.cpp` | Tests unitarios con NNG mockeado. | +| `CppClientIntegrationTests.cpp` | Test de conexion real contra Unreal/ProjectAirSim. | +| `FakeNNGI.h` / `FakeNNGI.cpp` | Implementacion fake de NNG para los unit tests. | + diff --git a/core_sim/src/CMakeLists.txt b/core_sim/src/CMakeLists.txt index fb65914d..9738c889 100644 --- a/core_sim/src/CMakeLists.txt +++ b/core_sim/src/CMakeLists.txt @@ -155,7 +155,7 @@ else() ${TARGET_NAME} PRIVATE nng - crypto # Client authorization + ${OPENSSL_CRYPTO_LIBRARY} # Client authorization ) endif() diff --git a/core_sim/src/client_authorization_linux.cpp b/core_sim/src/client_authorization_linux.cpp index f96c9394..04ce0d9d 100644 --- a/core_sim/src/client_authorization_linux.cpp +++ b/core_sim/src/client_authorization_linux.cpp @@ -5,7 +5,8 @@ #ifdef __linux__ -#warning Setting OPENSSL to v1.1.0 compatibility mode--upgrade to v3 mode when possible +// Use OpenSSL v1.1.0 compatibility mode until this implementation is updated +// for OpenSSL v3 APIs. #define OPENSSL_API_COMPAT 0x10100000L #include diff --git a/core_sim/src/sensors/camera.cpp b/core_sim/src/sensors/camera.cpp index 5be3d16f..1b6140e8 100644 --- a/core_sim/src/sensors/camera.cpp +++ b/core_sim/src/sensors/camera.cpp @@ -572,16 +572,37 @@ std::vector Camera::Impl::RunOnnxModelOnImages(ImageMessage imgMsg) { try { if (!camera_settings.post_process_model_settings.session_initialized) { logger_.LogVerbose(name_, "[%s] Entered onnx section'.", id_.c_str()); + auto throw_on_onnx_status = [](OrtStatus* status, + const char* provider_name) { + if (status == nullptr) { + return; + } + + std::string error_message = + std::string("Failed to add ONNX execution provider ") + + provider_name + ": " + Ort::GetApi().GetErrorMessage(status); + Ort::GetApi().ReleaseStatus(status); + throw Error(error_message); + }; + if (camera_settings.post_process_model_settings.execution_provider == "cuda") { logger_.LogVerbose(name_, "Trying to add CUDA EP since it is enabled."); - OrtSessionOptionsAppendExecutionProvider_CUDA(onnx_.session_options, 0); + throw_on_onnx_status( + OrtSessionOptionsAppendExecutionProvider_CUDA( + onnx_.session_options, 0), + "CUDA"); logger_.LogVerbose(name_, "onnx CUDA session declared"); } else if (camera_settings.post_process_model_settings .execution_provider == "tensorrt") { - OrtSessionOptionsAppendExecutionProvider_Tensorrt(onnx_.session_options, - 0); - OrtSessionOptionsAppendExecutionProvider_CUDA(onnx_.session_options, 0); + throw_on_onnx_status( + OrtSessionOptionsAppendExecutionProvider_Tensorrt( + onnx_.session_options, 0), + "TensorRT"); + throw_on_onnx_status( + OrtSessionOptionsAppendExecutionProvider_CUDA( + onnx_.session_options, 0), + "CUDA"); logger_.LogVerbose(name_, "onnx TensorRT session declared"); } logger_.LogVerbose(name_, "onnx Creating session"); diff --git a/core_sim/src/topic_manager.cpp b/core_sim/src/topic_manager.cpp index 0e868237..e6018e72 100644 --- a/core_sim/src/topic_manager.cpp +++ b/core_sim/src/topic_manager.cpp @@ -260,11 +260,11 @@ void TopicManager::Impl::Start() { // drop the send after this specified send_timeout to prevent the dispatcher // queue from stacking up const int send_timeout = 1000 / 60; // 16.7 ms = 60 FPS - rv = nng_setopt_ms(topic_socket_, NNG_OPT_SENDTIMEO, send_timeout); + rv = nng_socket_set_ms(topic_socket_, NNG_OPT_SENDTIMEO, send_timeout); if (rv != 0) { auto errno_str = nng_strerror(rv); log_.LogError(Constant::Component::topic_manager, - "nng_setopt_ms failed with '%s'.", errno_str); + "nng_socket_set_ms failed with '%s'.", errno_str); throw Error("Error setting send timeout on server socket."); } diff --git a/core_sim/test/CMakeLists.txt b/core_sim/test/CMakeLists.txt index 08343042..f1515561 100644 --- a/core_sim/test/CMakeLists.txt +++ b/core_sim/test/CMakeLists.txt @@ -130,7 +130,7 @@ else() gtest_main core_sim nng - crypto # Client authorization + ${OPENSSL_CRYPTO_LIBRARY} # Client authorization ) endif() diff --git a/docs/client_setup.md b/docs/client_setup.md index 084b7f68..d6ca1356 100644 --- a/docs/client_setup.md +++ b/docs/client_setup.md @@ -162,7 +162,47 @@ For more details about using the client to connect, send/receive signals, etc, s ## C++ Client -Currently, only Python clients are supported. +Project AirSim also includes a native C++17 client under `client/cpp/`. + +The C++ client uses the following to communicate with the Project AirSim +simulation server: + +- A C++17 compiler +- CMake 3.20 or newer +- NNG-based transport through the `NNGI` wrapper +- The `ProjectAirSimMessageLib` message serialization library +- The high-level `ProjectAirsimClient` API library + +Build the C++ client from the repository root: + +```bash +./build.sh cpp_client_debug +./build.sh cpp_client_release +``` + +On Windows: + +```bat +build.cmd cpp_client_debug +build.cmd cpp_client_release +``` + +Compiled Linux libraries and example binaries are placed in: + +```text +client/cpp/build_linux/Debug/ +client/cpp/build_linux/Release/ +``` + +Windows library artifacts are placed in: + +```text +client\cpp\libraries\x64\Debug\ +client\cpp\libraries\x64\Release\ +``` + +For setup details, binary locations, and usage examples, see +**[Project AirSim C++ Client](cpp_client.md)**. --- diff --git a/docs/cpp_client.md b/docs/cpp_client.md new file mode 100644 index 00000000..39f1d9fe --- /dev/null +++ b/docs/cpp_client.md @@ -0,0 +1,293 @@ +# Project AirSim C++ Client + +The Project AirSim C++ client is a native client library for connecting to a +running Project AirSim simulation server. It provides the same main programming +model as the Python client--a `Client`, a `World`, and robot-specific objects +such as `Drone`--for applications that need a C++17 interface. + +The C++ client source is in `client/cpp/`. + +## Client layout + +The main C++ client components are: + +| Path | Purpose | +| --- | --- | +| `client/cpp/ProjectAirsimClientLib/` | High-level client API for connecting to the server, loading worlds, and controlling robots. | +| `client/cpp/ProjectAirSimMessageLib/` | Message serialization helpers used by the client. | +| `client/cpp/NNGI/` | NNG-based transport wrapper. | +| `client/cpp/example_user_apps/HelloDrone/` | Minimal example application. | +| `client/cpp/example_user_apps/CppClientScenarios/` | Scenario-oriented example application. | +| `client/cpp/example_user_apps/UserScenarioTemplate/` | Starting point for custom C++ user scenarios. | +| `client/cpp/scripts/` | Linux helper scripts for building and running scenario examples. | + +The public API headers are under: + +```text +client/cpp/ProjectAirsimClientLib/Include/ProjectAirsimClient/ +``` + +Most applications should include the aggregate header: + +```cpp +#include +``` + +All public client types are in the `microsoft::projectairsim::client` +namespace. The examples commonly use this alias: + +```cpp +namespace pasc = microsoft::projectairsim::client; +``` + +## Prerequisites + +To build the C++ client, install: + +- CMake 3.20 or newer +- A C++17 compiler +- Ninja on Linux, when using the repository `build.sh` targets +- Visual Studio 2022 C++ build tools on Windows, when using `build.cmd` + +The Linux CMake build fetches third-party dependencies such as NNG, +`nlohmann-json`, and msgpack on first configure. Eigen3 is used from the system +when available, or fetched automatically. + +## Building on Linux + +From the repository root, build the C++ client with: + +```bash +./build.sh cpp_client_debug +./build.sh cpp_client_release +``` + +The C++ client is also built as part of the simulation library targets: + +```bash +./build.sh simlibs_debug +./build.sh simlibs_release +``` + +Build outputs are placed under: + +| Build | Output directory | +| --- | --- | +| Debug | `client/cpp/build_linux/Debug/` | +| Release | `client/cpp/build_linux/Release/` | + +The main static libraries are generated in the selected build directory: + +```text +libProjectAirsimClient.a +libProjectAirSimMessageLib.a +libNNGI.a +``` + +Example executables are generated in the same directory, including: + +```text +hello_drone +cpp_client_scenarios +user_scenario_template +``` + +You can also build the client manually with CMake: + +```bash +cmake -S client/cpp -B client/cpp/build_linux/Debug -DCMAKE_BUILD_TYPE=Debug +cmake --build client/cpp/build_linux/Debug -j$(nproc) +``` + +## Building on Windows + +From a Windows command prompt, use the repository build wrapper: + +```bat +build.cmd cpp_client_debug +build.cmd cpp_client_release +``` + +The Windows Visual Studio projects place C++ client library artifacts under: + +```text +client\cpp\libraries\x64\Debug\ +client\cpp\libraries\x64\Release\ +``` + +The `HelloDrone` example executable is placed under: + +```text +client\cpp\example_user_apps\HelloDrone\x64\Debug\ +client\cpp\example_user_apps\HelloDrone\x64\Release\ +``` + +When running `hello_drone.exe`, make sure the corresponding +`client\cpp\libraries\x64\\` directory is on `PATH`, or copy the +required DLLs next to the executable. + +## Running HelloDrone + +Start a Project AirSim simulation environment first, either from Unreal Editor +with the Project AirSim plugin or from a packaged environment such as Blocks. + +The `hello_drone` example connects to the simulation server, loads +`scene_basic_drone.jsonc`, arms `Drone1`, takes off, moves up, lands, and +disconnects. + +On Linux: + +```bash +./client/cpp/build_linux/Debug/hello_drone \ + --simhost 127.0.0.1 \ + --simconfig client/python/example_user_scripts/sim_config +``` + +On Windows: + +```bat +client\cpp\example_user_apps\HelloDrone\x64\Debug\hello_drone.exe ^ + --simhost 127.0.0.1 ^ + --simconfig client\python\example_user_scripts\sim_config +``` + +The `--simconfig` directory must contain the scene file and related robot +configuration files. The repository includes ready-to-run examples in: + +```text +client/python/example_user_scripts/sim_config/ +``` + +## Creating a C++ application + +For a custom Linux CMake application, build the client first and then link +against the generated libraries. The simplest in-repository workflow is to add a +new executable target to `client/cpp/CMakeLists.txt`, link it to +`ProjectAirsimClient`, and rebuild. + +For example: + +```cmake +add_executable(my_drone_app + example_user_apps/MyDroneApp/MyDroneApp.cpp +) + +target_link_libraries(my_drone_app + PRIVATE + ProjectAirsimClient +) + +if(NOT WIN32) + target_link_libraries(my_drone_app PRIVATE Threads::Threads) +endif() +``` + +Then build the target: + +```bash +cmake --build client/cpp/build_linux/Debug --target my_drone_app -j$(nproc) +``` + +For custom scenarios, start from: + +```text +client/cpp/example_user_apps/UserScenarioTemplate/UserScenarioTemplate.cpp +``` + +The helper script can build and run a scenario target from +`client/cpp/build_local`: + +```bash +cmake -S client/cpp -B client/cpp/build_local -DCMAKE_BUILD_TYPE=Debug +./client/cpp/scripts/run_cpp_user_scenario.sh --target user_scenario_template +``` + +## Minimal client code + +The following shows the basic C++ client flow. See +`client/cpp/example_user_apps/HelloDrone/HelloDrone.cpp` for a complete example +with argument parsing and error handling. + +```cpp +#include + +#include +#include + +namespace pasc = microsoft::projectairsim::client; + +int main() { + auto client = std::make_shared(); + if (client->Connect("127.0.0.1") != pasc::Status::OK) { + return 1; + } + + auto world = std::make_shared(); + if (world->Initialize(client, + "scene_basic_drone.jsonc", + "client/python/example_user_scripts/sim_config", + 2.0f) != pasc::Status::OK) { + return 1; + } + + auto drone = std::make_shared(); + if (drone->Initialize(client, world, "Drone1") != pasc::Status::OK) { + return 1; + } + + bool ok = false; + drone->EnableAPIControl(&ok); + drone->Arm(&ok); + + drone->TakeoffAsync().Wait(); + drone->MoveByVelocityAsync(0.0f, 0.0f, -1.0f, 4.0).Wait(); + drone->LandAsync().Wait(); + + drone->Disarm(&ok); + drone->DisableAPIControl(&ok); + client->Disconnect(); + + return 0; +} +``` + +## Async operations + +Movement APIs return `AsyncResult`. Call `Wait()` to block until the operation is +complete and retrieve its `Status`. + +```cpp +auto takeoff = drone->TakeoffAsync(); +pasc::Status status = takeoff.Wait(); +``` + +You can also poll for completion, but still call `Wait()` afterward to retrieve +the final status: + +```cpp +auto move = drone->MoveByVelocityAsync(0.0f, 0.0f, -1.0f, 4.0); + +while (!move.FIsDone()) { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); +} + +pasc::Status status = move.Wait(); +``` + +## More examples + +Additional example applications are available under: + +```text +client/cpp/example_user_apps/ +``` + +The scenario runner scripts under `client/cpp/scripts/` demonstrate common +flight, sensor, rover, environment actor, lidar, radar, and battery workflows. + +--- + +Copyright (C) Microsoft Corporation. +Copyright (C) 2025 IAMAI CONSULTING CORP + +MIT License. All rights reserved. diff --git a/docs/development/use_source.md b/docs/development/use_source.md index 7e6bf54f..8d749bb5 100644 --- a/docs/development/use_source.md +++ b/docs/development/use_source.md @@ -77,13 +77,16 @@ On Linux, run the `build.sh` shell script. ``` all = Clean + Build + Test + Package everything -clean = Clean sim libs + Blocks build files +clean = Clean sim libs, C++ client + Blocks build files simlibs_debug = Build + Package sim libs for Debug simlibs_release = Build + Package sim libs for Release test_simlibs_debug = Test sim libs for Debug test_simlibs_release = Test sim libs for Release +cpp_client_debug = Build C++ client artifacts for Debug +cpp_client_release = Build C++ client artifacts for Release + blocks_debuggame = Build Plugin + Blocks for DebugGame (uses Debug sim libs) blocks_development = Build Plugin + Blocks for Development (uses Release sim libs) @@ -92,6 +95,8 @@ package_blocks_debuggame = Package stand-alone Blocks environment executable for package_blocks_development = Package stand-alone Blocks environment executable for Development ``` +On Windows, the C++ client build reuses the existing ProjectAirsimClientLib and HelloDrone solution artifacts under `client/cpp/`. + The sim lib components and unit test executables are built in the `projectairsim/build/` folder using CMake, and are automatically copied to the Unreal Blocks environment folder to be ready for building the plugin. The Plugin components are built in the `projectairsim/unreal/Blocks/` folder using the Unreal Engine's build system. diff --git a/docs/index.md b/docs/index.md index 70cb555a..0311f857 100644 --- a/docs/index.md +++ b/docs/index.md @@ -89,6 +89,7 @@ physics/matlab_physics.md :caption: APIs & Programmatic Control api.md +cpp_client.md ros/ros.md ``` @@ -97,4 +98,4 @@ ros/ros.md :caption: Python Client API Reference Python Client API Reference -``` \ No newline at end of file +``` diff --git a/physics/test/CMakeLists.txt b/physics/test/CMakeLists.txt index 330cf131..f35684a0 100644 --- a/physics/test/CMakeLists.txt +++ b/physics/test/CMakeLists.txt @@ -75,7 +75,7 @@ target_link_libraries( gtest_main physics nng - crypto # Client authorization + ${OPENSSL_CRYPTO_LIBRARY} # Client authorization core_sim lvmon ) diff --git a/rendering/scene/test/CMakeLists.txt b/rendering/scene/test/CMakeLists.txt index 0dd82678..d6ab2117 100644 --- a/rendering/scene/test/CMakeLists.txt +++ b/rendering/scene/test/CMakeLists.txt @@ -72,7 +72,7 @@ target_link_libraries( core_sim nng lvmon - crypto # Client authorization + ${OPENSSL_CRYPTO_LIBRARY} # Client authorization ) endif() diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 9a07e4ac..3557eb45 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -15,4 +15,5 @@ # # --------------------------------------------------------------------------------------------------------------------- -# add_subdirectory(standalone_sim) \ No newline at end of file +# add_subdirectory(standalone_sim) +add_subdirectory(standalone_sim) diff --git a/samples/standalone_sim/CMakeLists.txt b/samples/standalone_sim/CMakeLists.txt index b6262679..42eee51b 100644 --- a/samples/standalone_sim/CMakeLists.txt +++ b/samples/standalone_sim/CMakeLists.txt @@ -90,6 +90,7 @@ else() multirotor_api rendering_scene nng - crypto # Client authorization + ${OPENSSL_CRYPTO_LIBRARY} # Client authorization + dl # Required by static libcrypto (dlopen/dlsym) ) endif() diff --git a/vehicle_apis/multirotor_api/test/CMakeLists.txt b/vehicle_apis/multirotor_api/test/CMakeLists.txt index bc181255..5944014e 100644 --- a/vehicle_apis/multirotor_api/test/CMakeLists.txt +++ b/vehicle_apis/multirotor_api/test/CMakeLists.txt @@ -74,7 +74,7 @@ target_link_libraries( nng mavlinkcom lvmon - crypto # Client authorization + ${OPENSSL_CRYPTO_LIBRARY} # Client authorization ) endif() diff --git a/vehicle_apis/rover_api/test/CMakeLists.txt b/vehicle_apis/rover_api/test/CMakeLists.txt index 12982c07..4b57c860 100644 --- a/vehicle_apis/rover_api/test/CMakeLists.txt +++ b/vehicle_apis/rover_api/test/CMakeLists.txt @@ -71,7 +71,7 @@ target_link_libraries( core_sim nng lvmon - crypto # Client authorization + ${OPENSSL_CRYPTO_LIBRARY} # Client authorization ) endif() From 0b9090f84de466c90a235b0408d1580b27ac7a12 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Sun, 12 Jul 2026 16:43:40 -0300 Subject: [PATCH 2/2] Add ROS2 C++ bridge and its tests --------- Co-authored-by: aurebidart Co-authored-by: Lucas Scheinkerman Co-authored-by: Marina Gloc Co-authored-by: jonyMarino --- docs/index.md | 2 +- docs/ros/ros2.md | 351 ++++ ros/projectairsim_ros2_cpp/CMakeLists.txt | 206 +++ .../action/MoveOnPath.action | 13 + .../ros2_conversion_utils.hpp | 378 +++++ .../msg/RadarReturn.msg | 5 + ros/projectairsim_ros2_cpp/msg/RadarScan.msg | 2 + ros/projectairsim_ros2_cpp/msg/RadarTrack.msg | 7 + .../msg/RadarTracks.msg | 2 + ros/projectairsim_ros2_cpp/package.xml | 30 + .../src/projectairsim_ros2_cpp_node.cpp | 1412 +++++++++++++++++ ros/projectairsim_ros2_cpp/srv/Arm.srv | 3 + ros/projectairsim_ros2_cpp/srv/ArmGroup.srv | 4 + .../srv/CancelAllRequests.srv | 2 + .../srv/CreateVoxelGrid.srv | 11 + ros/projectairsim_ros2_cpp/srv/Disarm.srv | 3 + .../srv/DisarmGroup.srv | 4 + .../srv/GetClientInfo.srv | 5 + ros/projectairsim_ros2_cpp/srv/GetClock.srv | 2 + ros/projectairsim_ros2_cpp/srv/GetDrones.srv | 3 + .../srv/GetOriginGeoPoint.srv | 4 + .../srv/GetSegmentationIDByName.srv | 6 + .../srv/GetSegmentationIDMap.srv | 4 + .../srv/GetTopicInfo.srv | 4 + ros/projectairsim_ros2_cpp/srv/Land.srv | 3 + ros/projectairsim_ros2_cpp/srv/LandGroup.srv | 4 + ros/projectairsim_ros2_cpp/srv/LoadScene.srv | 4 + ros/projectairsim_ros2_cpp/srv/MoveOnPath.srv | 11 + .../srv/MoveToPosition.srv | 13 + .../srv/OccupancyGrid.srv | 12 + ros/projectairsim_ros2_cpp/srv/Publish.srv | 5 + ros/projectairsim_ros2_cpp/srv/RawRequest.srv | 8 + ros/projectairsim_ros2_cpp/srv/Reset.srv | 3 + .../srv/SetObjectMaterial.srv | 5 + .../srv/SetObjectTexture.srv | 9 + .../srv/SetSegmentationIDByName.srv | 7 + .../srv/SwapObjectTexture.srv | 5 + ros/projectairsim_ros2_cpp/srv/Takeoff.srv | 3 + .../srv/TakeoffGroup.srv | 4 + .../srv/Unsubscribe.srv | 5 + .../ros2_bridge_surface_regression_test.cpp | 119 ++ .../test/ros2_conversion_utils_test.cpp | 168 ++ ...2_generated_interfaces_regression_test.cpp | 133 ++ .../test/ros2_interfaces_test.cpp | 421 +++++ .../test/ros2_unreal_integration_test.cpp | 360 +++++ 45 files changed, 3764 insertions(+), 1 deletion(-) create mode 100644 docs/ros/ros2.md create mode 100644 ros/projectairsim_ros2_cpp/CMakeLists.txt create mode 100644 ros/projectairsim_ros2_cpp/action/MoveOnPath.action create mode 100644 ros/projectairsim_ros2_cpp/include/projectairsim_ros2_cpp/ros2_conversion_utils.hpp create mode 100644 ros/projectairsim_ros2_cpp/msg/RadarReturn.msg create mode 100644 ros/projectairsim_ros2_cpp/msg/RadarScan.msg create mode 100644 ros/projectairsim_ros2_cpp/msg/RadarTrack.msg create mode 100644 ros/projectairsim_ros2_cpp/msg/RadarTracks.msg create mode 100644 ros/projectairsim_ros2_cpp/package.xml create mode 100644 ros/projectairsim_ros2_cpp/src/projectairsim_ros2_cpp_node.cpp create mode 100644 ros/projectairsim_ros2_cpp/srv/Arm.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/ArmGroup.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/CancelAllRequests.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/CreateVoxelGrid.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/Disarm.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/DisarmGroup.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/GetClientInfo.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/GetClock.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/GetDrones.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/GetOriginGeoPoint.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/GetSegmentationIDByName.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/GetSegmentationIDMap.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/GetTopicInfo.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/Land.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/LandGroup.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/LoadScene.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/MoveOnPath.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/MoveToPosition.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/OccupancyGrid.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/Publish.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/RawRequest.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/Reset.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/SetObjectMaterial.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/SetObjectTexture.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/SetSegmentationIDByName.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/SwapObjectTexture.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/Takeoff.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/TakeoffGroup.srv create mode 100644 ros/projectairsim_ros2_cpp/srv/Unsubscribe.srv create mode 100644 ros/projectairsim_ros2_cpp/test/ros2_bridge_surface_regression_test.cpp create mode 100644 ros/projectairsim_ros2_cpp/test/ros2_conversion_utils_test.cpp create mode 100644 ros/projectairsim_ros2_cpp/test/ros2_generated_interfaces_regression_test.cpp create mode 100644 ros/projectairsim_ros2_cpp/test/ros2_interfaces_test.cpp create mode 100644 ros/projectairsim_ros2_cpp/test/ros2_unreal_integration_test.cpp diff --git a/docs/index.md b/docs/index.md index 0311f857..b74aed9a 100644 --- a/docs/index.md +++ b/docs/index.md @@ -90,7 +90,7 @@ physics/matlab_physics.md api.md cpp_client.md -ros/ros.md +ros/ros2.md ``` ```{toctree} diff --git a/docs/ros/ros2.md b/docs/ros/ros2.md new file mode 100644 index 00000000..468d3a2a --- /dev/null +++ b/docs/ros/ros2.md @@ -0,0 +1,351 @@ +# Project AirSim ROS2 C++ Bridge + +Project AirSim provides a ROS2 C++ bridge package named +`projectairsim_ros2_cpp`. The package is located under: + +```text +ros/projectairsim_ros2_cpp +``` + +The bridge is an `rclcpp` node that connects to Project AirSim through the +Project AirSim C++ client API, discovers simulator topics, republishes supported +payloads as typed ROS2 messages, and exposes command services/actions. + +## Requirements + +The bridge has been built and tested with ROS2 Humble. Source your ROS2 +installation before building or running the node: + +```bash +source /opt/ros/humble/setup.bash +``` + +The package depends on: + +| Dependency | Used for | +|---|---| +| `ament_cmake` | ROS2 CMake package build | +| `rosidl_default_generators` | Custom service/action generation | +| `rclcpp` | ROS2 C++ node | +| `rclcpp_action` | `MoveOnPath` action server | +| `geometry_msgs` | poses and path goals | +| `nav_msgs` | occupancy grid service responses | +| `rosgraph_msgs` | simulation clock topic | +| `sensor_msgs` | sensor topics | +| `std_msgs` | topic info and raw JSON fallback topics | +| `tf2_ros` | TF broadcasting | + +## Build + +From the repository root: + +```bash +source /opt/ros/humble/setup.bash +cd ros +colcon build --packages-select projectairsim_ros2_cpp +source install/setup.bash +``` + +## Test + +The ROS2 C++ package includes C++ contract tests for the generated topics, +services, and action interfaces. + +```bash +source /opt/ros/humble/setup.bash +cd ros +colcon test --packages-select projectairsim_ros2_cpp +colcon test-result --all --verbose +``` + +## Run + +Start Project AirSim first. Then, from `ros`: + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash + +ros2 run projectairsim_ros2_cpp projectairsim_ros2_cpp_node --ros-args \ + -p scene_config:=scene_drone_sensors.jsonc \ + -p sim_config_path:=../client/python/example_user_scripts/sim_config +``` + +If Project AirSim is not reachable at `127.0.0.1`, set `address` to the host +running the simulator: + +```bash +ros2 run projectairsim_ros2_cpp projectairsim_ros2_cpp_node --ros-args \ + -p address:= \ + -p scene_config:=scene_drone_sensors.jsonc \ + -p sim_config_path:=../client/python/example_user_scripts/sim_config +``` + +If a scene is already loaded and should not be changed by the bridge, omit +`scene_config`. In that mode the node connects to the existing simulator scene, +adopts the discovered scene topic root, and starts topic and clock publishing +when Project AirSim reports topic information. + +## Parameters + +| Parameter | Default | Description | +|---|---|---| +| `address` | `127.0.0.1` | Project AirSim server address. Set this when the simulator runs on another host or network namespace. | +| `port_topics` | `8989` | Project AirSim pub-sub topic port. | +| `port_services` | `8990` | Project AirSim service request port. | +| `scene_config` | empty | Scene config file to load at startup. Relative names are resolved against `sim_config_path`. When empty, the bridge connects to the currently loaded scene. | +| `sim_config_path` | `client/python/example_user_scripts/sim_config` | Directory containing scene, robot, and environment config files. | +| `delay_after_load_sec` | `2.0` | Delay after loading a scene before topic discovery. | +| `projectairsim_topic_root` | `/Sim` | Native Project AirSim topic root to rewrite. | +| `ros_topic_root` | `/ProjectAirsim` | ROS topic root used when publishing bridged topics. | +| `publish_unmatched_as_json` | `false` | Publish unsupported Project AirSim topics as `std_msgs/String` JSON. | +| `publish_tf` | `true` | Broadcast TF transforms from `/actual_pose` and camera pose payloads. | +| `tf_world_frame_id` | `map` | Parent frame used for TF transforms. | +| `refresh_topics_period_sec` | `0.0` | Periodic topic discovery interval. `0.0` disables polling; use a positive value only if scenes/topics can change outside this node. | +| `publish_clock_period_sec` | `0.02` | Periodic `/clock` publish interval in seconds. `0.0` disables clock publishing. | +| `vehicle_name` | `Drone1` | Vehicle used for single-drone services/actions. | +| `service_root` | `/projectairsim` | Root namespace for command services and actions. | + +## Topics + +List ROS2 topics: + +```bash +ros2 topic list -t +``` + +The bridge subscribes to native Project AirSim topics such as `/Sim/...` and republishes them under `/ProjectAirsim/...` by default. + +Robot state topics use the vehicle name as `frame_id`; camera image and +camera-info topics use a unique sanitized camera frame so they can participate +in TF without colliding with the vehicle frame. + +Implemented typed topic conversions: + +| Project AirSim topic suffix | ROS2 topic suffix | ROS2 topic type | +|---|---|---| +| `/actual_pose` | `/actual_pose` | `geometry_msgs/msg/PoseStamped` | +| `/gps` | `/gps` | `sensor_msgs/msg/NavSatFix` | +| `/imu`, `/imu_kinematics` | `/imu` | `sensor_msgs/msg/Imu` | +| `/barometer` | `/barometer` | `sensor_msgs/msg/FluidPressure` | +| `/magnetometer` | `/magnetometer` | `sensor_msgs/msg/MagneticField` | +| `/lidar` | `/lidar` | `sensor_msgs/msg/PointCloud2` | +| `/radar_detections` | `/radar_detections` | `projectairsim_ros2_cpp/msg/RadarScan` | +| `/radar_tracks` | `/radar_tracks` | `projectairsim_ros2_cpp/msg/RadarTracks` | +| camera topics | same suffix | `sensor_msgs/msg/Image` | +| `*_camera_info` | `/camera_info` | `sensor_msgs/msg/CameraInfo` | + +The bridge also publishes: + +| Topic | Type | Description | +|---|---|---| +| `/projectairsim/topic_info` | `std_msgs/msg/String` | JSON list of Project AirSim topic paths from the first discovery pass after startup or scene load. | +| `/clock` | `rosgraph_msgs/msg/Clock` | Project AirSim simulation time from `World::GetSimTime()`. | +| `/tf` | `tf2_msgs/msg/TFMessage` | Vehicle and camera transforms when `publish_tf=true`. | + +To echo a topic: + +```bash +ros2 topic echo /ProjectAirsim//robots//actual_pose +``` + +If you want raw JSON fallback topics for unsupported Project AirSim topics: + +```bash +ros2 run projectairsim_ros2_cpp projectairsim_ros2_cpp_node --ros-args \ + -p publish_unmatched_as_json:=true +``` + +## Services + +List services: + +```bash +ros2 service list -t +``` + +Single-vehicle services use the configured `vehicle_name`, default `Drone1`: + +| Service | Type | +|---|---| +| `/projectairsim/Drone1/takeoff` | `projectairsim_ros2_cpp/srv/Takeoff` | +| `/projectairsim/Drone1/land` | `projectairsim_ros2_cpp/srv/Land` | +| `/projectairsim/Drone1/arm` | `projectairsim_ros2_cpp/srv/Arm` | +| `/projectairsim/Drone1/disarm` | `projectairsim_ros2_cpp/srv/Disarm` | +| `/projectairsim/Drone1/move_to_position` | `projectairsim_ros2_cpp/srv/MoveToPosition` | +| `/projectairsim/Drone1/move_on_path` | `projectairsim_ros2_cpp/srv/MoveOnPath` | + +Global, group, and scene services: + +| Service | Type | +|---|---| +| `/projectairsim/request` | `projectairsim_ros2_cpp/srv/RawRequest` | +| `/projectairsim/get_client_info` | `projectairsim_ros2_cpp/srv/GetClientInfo` | +| `/projectairsim/get_topic_info` | `projectairsim_ros2_cpp/srv/GetTopicInfo` | +| `/projectairsim/get_drones` | `projectairsim_ros2_cpp/srv/GetDrones` | +| `/projectairsim/publish` | `projectairsim_ros2_cpp/srv/Publish` | +| `/projectairsim/unsubscribe` | `projectairsim_ros2_cpp/srv/Unsubscribe` | +| `/projectairsim/cancel_all_requests` | `projectairsim_ros2_cpp/srv/CancelAllRequests` | +| `/projectairsim/takeoff_group` | `projectairsim_ros2_cpp/srv/TakeoffGroup` | +| `/projectairsim/land_group` | `projectairsim_ros2_cpp/srv/LandGroup` | +| `/projectairsim/arm_group` | `projectairsim_ros2_cpp/srv/ArmGroup` | +| `/projectairsim/disarm_group` | `projectairsim_ros2_cpp/srv/DisarmGroup` | +| `/projectairsim/reset` | `projectairsim_ros2_cpp/srv/Reset` | +| `/projectairsim/load_scene` | `projectairsim_ros2_cpp/srv/LoadScene` | +| `/projectairsim/create_voxel_grid` | `projectairsim_ros2_cpp/srv/CreateVoxelGrid` | +| `/projectairsim/occupancy_grid` | `projectairsim_ros2_cpp/srv/OccupancyGrid` | +| `/projectairsim/get_clock` | `projectairsim_ros2_cpp/srv/GetClock` | +| `/projectairsim/get_origin_geo_point` | `projectairsim_ros2_cpp/srv/GetOriginGeoPoint` | +| `/projectairsim/set_segmentation_id_by_name` | `projectairsim_ros2_cpp/srv/SetSegmentationIDByName` | +| `/projectairsim/get_segmentation_id_by_name` | `projectairsim_ros2_cpp/srv/GetSegmentationIDByName` | +| `/projectairsim/get_segmentation_id_map` | `projectairsim_ros2_cpp/srv/GetSegmentationIDMap` | +| `/projectairsim/set_object_material` | `projectairsim_ros2_cpp/srv/SetObjectMaterial` | +| `/projectairsim/set_object_texture` | `projectairsim_ros2_cpp/srv/SetObjectTexture` | +| `/projectairsim/swap_object_texture` | `projectairsim_ros2_cpp/srv/SwapObjectTexture` | + +Examples: + +```bash +ros2 service call /projectairsim/Drone1/arm projectairsim_ros2_cpp/srv/Arm \ + "{wait_on_last_task: true}" +``` + +```bash +ros2 service call /projectairsim/Drone1/takeoff projectairsim_ros2_cpp/srv/Takeoff \ + "{wait_on_last_task: true}" +``` + +```bash +ros2 service call /projectairsim/Drone1/move_to_position projectairsim_ros2_cpp/srv/MoveToPosition \ + "{x: 5.0, y: 0.0, z: -3.0, velocity: 2.0, timeout_sec: 10.0, drive_train_type: 0, yaw_is_rate: true, yaw: 0.0, lookahead: -1.0, adaptive_lookahead: 1.0, wait_on_last_task: true}" +``` + +```bash +ros2 service call /projectairsim/Drone1/land projectairsim_ros2_cpp/srv/Land \ + "{wait_on_last_task: true}" +``` + +Reload a scene at runtime: + +```bash +ros2 service call /projectairsim/load_scene projectairsim_ros2_cpp/srv/LoadScene \ + '{scene_file: "scene_drone_sensors.jsonc", is_primary_client: true}' +``` + +Use `is_primary_client: true` when this bridge should ask Project AirSim to load +the scene. Use `is_primary_client: false` when another client has already loaded +the scene and this bridge should attach to it. + +Create a voxel grid file: + +```bash +ros2 service call /projectairsim/create_voxel_grid projectairsim_ros2_cpp/srv/CreateVoxelGrid \ + '{position_x: 0.0, position_y: 0.0, position_z: -4.0, ncells_x: 20, ncells_y: 20, ncells_z: 10, resolution: 1.0, n_z_resolution: 10, output_file: "/tmp/projectairsim_voxel_grid.binvox"}' +``` + +Return a ROS2 occupancy grid. The 3D voxel grid is projected into a 2D +`nav_msgs/OccupancyGrid`; a cell is occupied if any voxel in its Z column is +occupied. + +```bash +ros2 service call /projectairsim/occupancy_grid projectairsim_ros2_cpp/srv/OccupancyGrid \ + "{position_x: 0.0, position_y: 0.0, position_z: -4.0, ncells_x: 20, ncells_y: 20, ncells_z: 10, res: 1.0, n_z_resolution: 10}" +``` + +Get simulation time in nanoseconds: + +```bash +ros2 service call /projectairsim/get_clock projectairsim_ros2_cpp/srv/GetClock "{}" +``` + +Get the scene origin geo point: + +```bash +ros2 service call /projectairsim/get_origin_geo_point projectairsim_ros2_cpp/srv/GetOriginGeoPoint "{}" +``` + +Call any Project AirSim C++ client request-backed API by method path and JSON +parameters: + +```bash +ros2 service call /projectairsim/request projectairsim_ros2_cpp/srv/RawRequest \ + '{method: "/Sim/GetBuildCommitHash", json_parameters: "{}"}' +``` + +`/projectairsim/request` is a generic escape hatch for Project AirSim server +request paths. Prefer typed ROS2 services when one exists. For generic calls, +use a method path and JSON payload supported by the Project AirSim server. + +## Actions + +The bridge provides a `MoveOnPath` action: + +| Action | Type | +|---|---| +| `/projectairsim/Drone1/move_on_path` | `projectairsim_ros2_cpp/action/MoveOnPath` | + +List actions: + +```bash +ros2 action list -t +``` + +Send a path goal: + +```bash +ros2 action send_goal /projectairsim/Drone1/move_on_path projectairsim_ros2_cpp/action/MoveOnPath \ + "{path: [{pose: {position: {x: 5.0, y: 0.0, z: -3.0}, orientation: {w: 1.0}}}, {pose: {position: {x: 5.0, y: 5.0, z: -3.0}, orientation: {w: 1.0}}}], velocity: 2.0, timeout_sec: 20.0, lookahead: -1.0, adaptive_lookahead: 1.0, drive_train_type: 0, yaw_is_rate: true, yaw: 0.0, wait_on_last_task: true}" +``` + +## Common Workflow + +Use three terminals. Run the ROS2 commands from the repository root unless +noted otherwise: + +1. Start Project AirSim. +2. Start the bridge: + +```bash +source /opt/ros/humble/setup.bash +cd ros +source install/setup.bash +ros2 run projectairsim_ros2_cpp projectairsim_ros2_cpp_node --ros-args \ + -p scene_config:=scene_drone_sensors.jsonc \ + -p sim_config_path:=../client/python/example_user_scripts/sim_config +``` + +3. Inspect and command from ROS2: + +```bash +source /opt/ros/humble/setup.bash +cd ros +source install/setup.bash +ros2 topic list -t +ros2 service list -t +ros2 action list -t +``` + +Then call services such as arm, takeoff, move, and land. + +## Resolved Limitations And Remaining Gaps + +The ROS2 C++ bridge provides typed ROS2 coverage for the high-priority areas +below: + +| Area | Status | +|---|---| +| Radar typed messages | Implemented as `projectairsim_ros2_cpp/msg/RadarScan` and `RadarTracks`. | +| Camera info topics | Implemented as `/camera_info` with `sensor_msgs/msg/CameraInfo`. | +| TF broadcasting | Implemented for vehicle pose and camera pose payloads; controlled by `publish_tf`. | +| Segmentation and object material/texture services | Implemented as typed ROS2 services. | + +Remaining gaps are narrower typed wrappers for Project AirSim request paths that +currently require `/projectairsim/request`, plus richer typed conversions for +specialized sensor/status payloads that do not yet have dedicated ROS message +contracts. + +--- + +Copyright (C) IAMAI CONSULTING CORP + +MIT License. All rights reserved. diff --git a/ros/projectairsim_ros2_cpp/CMakeLists.txt b/ros/projectairsim_ros2_cpp/CMakeLists.txt new file mode 100644 index 00000000..6cb56fe2 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 3.16) +project(projectairsim_ros2_cpp LANGUAGES C CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +option(PROJECTAIRSIM_ROS2_CPP_ENABLE_UNREAL_TESTS + "Build and register ROS2 C++ integration tests that require Unreal/Project AirSim and a running bridge" + OFF) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rosgraph_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rosidl_runtime_cpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +# ROS2 RMW implementations load the generated type support at runtime. +# Keep interface libraries shared even though the Project AirSim C++ client +# builds its third-party transport dependencies as static libraries. +set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries for ROS2 interfaces" FORCE) + +set(PROJECTAIRSIM_ROS2_CPP_INTERFACES + "msg/RadarReturn.msg" + "msg/RadarScan.msg" + "msg/RadarTrack.msg" + "msg/RadarTracks.msg" + "srv/Arm.srv" + "srv/ArmGroup.srv" + "srv/CancelAllRequests.srv" + "srv/CreateVoxelGrid.srv" + "srv/Disarm.srv" + "srv/DisarmGroup.srv" + "srv/GetClientInfo.srv" + "srv/GetClock.srv" + "srv/GetDrones.srv" + "srv/GetOriginGeoPoint.srv" + "srv/GetSegmentationIDByName.srv" + "srv/GetSegmentationIDMap.srv" + "srv/GetTopicInfo.srv" + "srv/Land.srv" + "srv/LandGroup.srv" + "srv/LoadScene.srv" + "srv/MoveOnPath.srv" + "srv/MoveToPosition.srv" + "srv/OccupancyGrid.srv" + "srv/Publish.srv" + "srv/RawRequest.srv" + "srv/Reset.srv" + "srv/SetObjectMaterial.srv" + "srv/SetObjectTexture.srv" + "srv/SetSegmentationIDByName.srv" + "srv/SwapObjectTexture.srv" + "srv/Takeoff.srv" + "srv/TakeoffGroup.srv" + "srv/Unsubscribe.srv" + "action/MoveOnPath.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${PROJECTAIRSIM_ROS2_CPP_INTERFACES} + DEPENDENCIES geometry_msgs nav_msgs std_msgs +) + +set(BUILD_SHARED_LIBS OFF CACHE BOOL "Build Project AirSim C++ client dependencies static" FORCE) + +get_filename_component(PROJECTAIRSIM_ROOT "${CMAKE_CURRENT_LIST_DIR}/../.." ABSOLUTE) +set(PROJECTAIRSIM_CPP_CLIENT_DIR + "${PROJECTAIRSIM_ROOT}/client/cpp" + CACHE PATH "Path to the Project AirSim C++ client source tree") + +if(NOT TARGET ProjectAirsimClient) + add_subdirectory( + "${PROJECTAIRSIM_CPP_CLIENT_DIR}" + "${CMAKE_CURRENT_BINARY_DIR}/projectairsim_cpp_client" + EXCLUDE_FROM_ALL) +endif() + +add_executable(projectairsim_ros2_cpp_node + src/projectairsim_ros2_cpp_node.cpp +) + +target_include_directories(projectairsim_ros2_cpp_node + PRIVATE + include +) + +target_link_libraries(projectairsim_ros2_cpp_node + ProjectAirsimClient +) + +ament_target_dependencies(projectairsim_ros2_cpp_node + geometry_msgs + nav_msgs + rclcpp + rclcpp_action + rosgraph_msgs + sensor_msgs + std_msgs + tf2_ros +) + +rosidl_get_typesupport_target(cpp_typesupport_target + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +target_link_libraries(projectairsim_ros2_cpp_node + "${cpp_typesupport_target}" +) + +install(TARGETS projectairsim_ros2_cpp_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(ros2_interfaces_test + test/ros2_interfaces_test.cpp + ) + if(TARGET ros2_interfaces_test) + ament_target_dependencies(ros2_interfaces_test + geometry_msgs + nav_msgs + rosgraph_msgs + rosidl_runtime_cpp + sensor_msgs + std_msgs + tf2_ros + ) + target_link_libraries(ros2_interfaces_test + "${cpp_typesupport_target}" + ) + endif() + + ament_add_gtest(ros2_bridge_surface_regression_test + test/ros2_bridge_surface_regression_test.cpp + ) + + ament_add_gtest(ros2_generated_interfaces_regression_test + test/ros2_generated_interfaces_regression_test.cpp + ) + if(TARGET ros2_generated_interfaces_regression_test) + ament_target_dependencies(ros2_generated_interfaces_regression_test + geometry_msgs + rosidl_runtime_cpp + std_msgs + ) + target_link_libraries(ros2_generated_interfaces_regression_test + "${cpp_typesupport_target}" + ) + endif() + + ament_add_gtest(ros2_conversion_utils_test + test/ros2_conversion_utils_test.cpp + ) + if(TARGET ros2_conversion_utils_test) + target_include_directories(ros2_conversion_utils_test + PRIVATE + include + ) + ament_target_dependencies(ros2_conversion_utils_test + geometry_msgs + sensor_msgs + rosidl_runtime_cpp + ) + target_link_libraries(ros2_conversion_utils_test + ProjectAirsimClient + "${cpp_typesupport_target}" + ) + endif() + + if(PROJECTAIRSIM_ROS2_CPP_ENABLE_UNREAL_TESTS) + ament_add_gtest(ros2_unreal_integration_test + test/ros2_unreal_integration_test.cpp + TIMEOUT 900 + ) + if(TARGET ros2_unreal_integration_test) + ament_target_dependencies(ros2_unreal_integration_test + geometry_msgs + rclcpp + rosgraph_msgs + sensor_msgs + ) + target_link_libraries(ros2_unreal_integration_test + "${cpp_typesupport_target}" + ) + set_tests_properties(ros2_unreal_integration_test + PROPERTIES + LABELS "unreal;integration" + ) + endif() + endif() +endif() + +ament_export_include_directories(include) +ament_package() diff --git a/ros/projectairsim_ros2_cpp/action/MoveOnPath.action b/ros/projectairsim_ros2_cpp/action/MoveOnPath.action new file mode 100644 index 00000000..c729b593 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/action/MoveOnPath.action @@ -0,0 +1,13 @@ +geometry_msgs/PoseStamped[] path +float64 velocity +float32 timeout_sec +float32 lookahead +float32 adaptive_lookahead +int32 drive_train_type +bool yaw_is_rate +float32 yaw +bool wait_on_last_task +--- +bool success +--- +string current_point_on_path diff --git a/ros/projectairsim_ros2_cpp/include/projectairsim_ros2_cpp/ros2_conversion_utils.hpp b/ros/projectairsim_ros2_cpp/include/projectairsim_ros2_cpp/ros2_conversion_utils.hpp new file mode 100644 index 00000000..07efa0ce --- /dev/null +++ b/ros/projectairsim_ros2_cpp/include/projectairsim_ros2_cpp/ros2_conversion_utils.hpp @@ -0,0 +1,378 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "json.hpp" +#include "msgpack.hpp" +#include "projectairsim_ros2_cpp/msg/radar_scan.hpp" +#include "projectairsim_ros2_cpp/msg/radar_tracks.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" + +namespace projectairsim_ros2_cpp::bridge { + +using json = nlohmann::json; + +constexpr double kPi = 3.14159265358979323846; + +inline bool EndsWith(const std::string& value, const std::string& suffix) { + return value.size() >= suffix.size() && + value.compare(value.size() - suffix.size(), suffix.size(), suffix) == 0; +} + +inline bool StartsWithPathPrefix(const std::string& value, + const std::string& prefix) { + return value == prefix || + (value.size() > prefix.size() && + value.compare(0, prefix.size(), prefix) == 0 && + value[prefix.size()] == '/'); +} + +inline std::string ReplaceSuffix(std::string value, const std::string& suffix, + const std::string& replacement) { + if (EndsWith(value, suffix)) { + value.replace(value.size() - suffix.size(), suffix.size(), replacement); + } + return value; +} + +inline std::vector SplitPath(const std::string& path) { + std::vector parts; + size_t start = 0; + while (start < path.size()) { + while (start < path.size() && path[start] == '/') { + ++start; + } + if (start >= path.size()) break; + + const auto end = path.find('/', start); + parts.push_back(path.substr(start, end == std::string::npos ? end : end - start)); + if (end == std::string::npos) break; + start = end + 1; + } + return parts; +} + +inline std::string SanitizeFrameId(std::string frame) { + if (!frame.empty() && frame.front() == '/') frame.erase(frame.begin()); + std::replace(frame.begin(), frame.end(), '/', '_'); + return frame.empty() ? "map" : frame; +} + +inline std::string TopicToFrameId(const std::string& topic) { + const auto normalized_topic = ReplaceSuffix(topic, "/imu_kinematics", "/imu"); + const auto parts = SplitPath(normalized_topic); + + for (size_t i = 0; i + 1 < parts.size(); ++i) { + if (parts[i] != "robots") continue; + return parts[i + 1]; + } + + return SanitizeFrameId(normalized_topic); +} + +inline std::string CameraFrameId(const std::string& topic) { + return SanitizeFrameId(ReplaceSuffix(topic, "_info", "")); +} + +inline std::string CameraInfoRosTopic(const std::string& image_ros_topic) { + return image_ros_topic + "/camera_info"; +} + +inline std::string CameraImageTopicFromInfoTopic(const std::string& topic) { + return ReplaceSuffix(topic, "_info", ""); +} + +inline std::string ParentTopicFromProjectAirSimTopic( + const std::string& topic, const std::string& topic_root) { + if (!StartsWithPathPrefix(topic, topic_root)) return ""; + + const auto parts = SplitPath(topic); + const auto root_parts = SplitPath(topic_root); + if (parts.size() <= root_parts.size() + 1) return ""; + if (parts[root_parts.size()].empty() || parts[root_parts.size()].front() == '$') { + return ""; + } + + std::string parent_topic = topic_root; + if (!parent_topic.empty() && parent_topic.back() == '/') { + parent_topic.pop_back(); + } + parent_topic += "/" + parts[root_parts.size()]; + return parent_topic; +} + +inline bool IsSceneParentTopic(const std::string& topic, + const std::string& topic_root) { + if (!StartsWithPathPrefix(topic, topic_root) || topic == topic_root) { + return false; + } + const auto parts = SplitPath(topic); + const auto root_parts = SplitPath(topic_root); + if (parts.size() <= root_parts.size()) return false; + return !parts[root_parts.size()].empty() && + parts[root_parts.size()].front() != '$'; +} + +inline json MsgpackToJson(const msgpack::object& object) { + switch (object.type) { + case msgpack::type::NIL: + return nullptr; + case msgpack::type::BOOLEAN: + return object.via.boolean; + case msgpack::type::POSITIVE_INTEGER: + return object.via.u64; + case msgpack::type::NEGATIVE_INTEGER: + return object.via.i64; + case msgpack::type::FLOAT32: + case msgpack::type::FLOAT64: + return object.via.f64; + case msgpack::type::STR: + return std::string(object.via.str.ptr, object.via.str.size); + case msgpack::type::BIN: + return std::string(object.via.bin.ptr, object.via.bin.size); + case msgpack::type::ARRAY: { + json array = json::array(); + for (uint32_t i = 0; i < object.via.array.size; ++i) { + array.push_back(MsgpackToJson(object.via.array.ptr[i])); + } + return array; + } + case msgpack::type::MAP: { + json map = json::object(); + for (uint32_t i = 0; i < object.via.map.size; ++i) { + const auto& key = object.via.map.ptr[i].key; + if (key.type != msgpack::type::STR) continue; + map[std::string(key.via.str.ptr, key.via.str.size)] = + MsgpackToJson(object.via.map.ptr[i].val); + } + return map; + } + default: + return nullptr; + } +} + +inline json UnpackProjectAirSimMessage(const std::string& payload) { + auto handle = msgpack::unpack(payload.data(), payload.size()); + return MsgpackToJson(handle.get()); +} + +inline double NumberOr(const json& object, const char* key, + double fallback = 0.0) { + if (!object.is_object() || !object.contains(key) || !object[key].is_number()) { + return fallback; + } + return object[key].get(); +} + +inline bool JsonToInt64(const json& value, std::int64_t* out) { + if (value.is_number_integer()) { + *out = value.get(); + return true; + } + if (value.is_number_unsigned()) { + const auto unsigned_value = value.get(); + if (unsigned_value > + static_cast(std::numeric_limits::max())) { + return false; + } + *out = static_cast(unsigned_value); + return true; + } + if (value.is_number_float()) { + *out = static_cast(std::llround(value.get())); + return true; + } + return false; +} + +inline bool ExtractSimTimeNanos(const json& value, std::int64_t* nanosec) { + if (JsonToInt64(value, nanosec)) return true; + if (!value.is_object()) return false; + + const auto sec_it = value.find("sec"); + const auto nsec_it = value.find("nanosec"); + if (sec_it != value.end() && nsec_it != value.end()) { + std::int64_t sec = 0; + std::int64_t nsec = 0; + if (JsonToInt64(*sec_it, &sec) && JsonToInt64(*nsec_it, &nsec)) { + *nanosec = sec * 1000000000LL + nsec; + return true; + } + } + + for (const auto* key : {"time_nanos", "sim_time_nanos", "nanosec", "nanos"}) { + auto it = value.find(key); + if (it != value.end() && JsonToInt64(*it, nanosec)) return true; + } + return false; +} + +inline std::vector ArrayNumbers(const json& value) { + std::vector numbers; + if (!value.is_array()) return numbers; + numbers.reserve(value.size()); + for (const auto& item : value) { + if (item.is_number()) numbers.push_back(item.get()); + } + return numbers; +} + +template +void FillFixedArray(const json& value, std::array* out) { + out->fill(0.0); + const auto numbers = ArrayNumbers(value); + for (size_t i = 0; i < std::min(numbers.size(), N); ++i) { + (*out)[i] = numbers[i]; + } +} + +inline geometry_msgs::msg::Vector3 ToRosVector3(const json& value) { + geometry_msgs::msg::Vector3 out; + const auto numbers = ArrayNumbers(value); + if (numbers.size() >= 3) { + out.x = numbers[0]; + out.y = -numbers[1]; + out.z = -numbers[2]; + } else if (value.is_object()) { + out.x = NumberOr(value, "x"); + out.y = -NumberOr(value, "y"); + out.z = -NumberOr(value, "z"); + } + return out; +} + +inline geometry_msgs::msg::Point ToRosPoint(const json& value) { + const auto vector = ToRosVector3(value); + geometry_msgs::msg::Point out; + out.x = vector.x; + out.y = vector.y; + out.z = vector.z; + return out; +} + +inline geometry_msgs::msg::Quaternion ToRosQuaternion(const json& value) { + geometry_msgs::msg::Quaternion out; + if (value.is_array() && value.size() >= 4) { + out.x = value[0].get(); + out.y = -value[1].get(); + out.z = -value[2].get(); + out.w = value[3].get(); + } else if (value.is_object()) { + out.x = NumberOr(value, "x"); + out.y = -NumberOr(value, "y"); + out.z = -NumberOr(value, "z"); + out.w = NumberOr(value, "w", 1.0); + } else { + out.w = 1.0; + } + return out; +} + +inline std::vector BytesFromJsonString(const json& value) { + std::vector bytes; + if (!value.is_string()) return bytes; + const auto data = value.get(); + bytes.assign(data.begin(), data.end()); + return bytes; +} + +inline float RadarAmplitudeFromRangeAndRcs(double range, double rcs_sqm) { + if (range <= 0.0 || rcs_sqm <= 0.0) return 0.0F; + return static_cast( + 10.0 * std::log10(rcs_sqm * 1000.0 / + (16.0 * kPi * kPi * std::pow(range, 4)) * 0.7 * + 15.0)); +} + +inline void AppendRadarReturnsFromJson( + const json& detections, projectairsim_ros2_cpp::msg::RadarScan* scan) { + if (!detections.is_array()) return; + scan->returns.reserve(scan->returns.size() + detections.size()); + for (const auto& detection : detections) { + projectairsim_ros2_cpp::msg::RadarReturn radar_return; + const auto range = NumberOr(detection, "range"); + radar_return.range = static_cast(range); + radar_return.azimuth = static_cast(NumberOr(detection, "azimuth")); + radar_return.elevation = static_cast(NumberOr(detection, "elevation")); + radar_return.doppler_velocity = + static_cast(NumberOr(detection, "velocity")); + radar_return.amplitude = + RadarAmplitudeFromRangeAndRcs(range, NumberOr(detection, "rcs_sqm")); + scan->returns.push_back(radar_return); + } +} + +inline void AppendRadarTracksFromJson( + const json& tracks, projectairsim_ros2_cpp::msg::RadarTracks* radar_tracks) { + if (!tracks.is_array()) return; + radar_tracks->tracks.reserve(radar_tracks->tracks.size() + tracks.size()); + for (const auto& track : tracks) { + projectairsim_ros2_cpp::msg::RadarTrack ros_track; + ros_track.id = static_cast(NumberOr(track, "id")); + ros_track.position = ToRosVector3(track.value("position_est", json::array())); + ros_track.velocity = ToRosVector3(track.value("velocity_est", json::array())); + ros_track.acceleration = ToRosVector3(track.value("accel_est", json::array())); + ros_track.azimuth = static_cast(NumberOr(track, "azimuth_est")); + ros_track.elevation = static_cast(NumberOr(track, "elevation_est")); + ros_track.range = static_cast(NumberOr(track, "range_est")); + radar_tracks->tracks.push_back(ros_track); + } +} + +inline void PopulateCameraInfoFromJson(const json& msg, + sensor_msgs::msg::CameraInfo* camera_info) { + camera_info->width = static_cast(NumberOr(msg, "width")); + camera_info->height = static_cast(NumberOr(msg, "height")); + camera_info->distortion_model = msg.value("distortion_model", std::string()); + camera_info->d = ArrayNumbers(msg.value("distortion_params", json::array())); + FillFixedArray(msg.value("intrinsic_camera_matrix", json::array()), &camera_info->k); + FillFixedArray(msg.value("rectification_matrix", json::array()), &camera_info->r); + FillFixedArray(msg.value("projection_matrix", json::array()), &camera_info->p); +} + +inline bool PopulateImagePayloadFromJson(const json& msg, + sensor_msgs::msg::Image* image) { + image->height = static_cast(NumberOr(msg, "height")); + image->width = static_cast(NumberOr(msg, "width")); + image->is_bigendian = static_cast(NumberOr(msg, "big_endian")); + const auto encoding = msg.value("encoding", std::string()); + if (encoding == "BGR") { + image->encoding = "bgr8"; + image->data = BytesFromJsonString(msg.value("data", "")); + image->step = 3 * image->width; + return true; + } + if (encoding == "16UC1") { + image->encoding = "mono16"; + image->data = BytesFromJsonString(msg.value("data", "")); + image->step = 2 * image->width; + return true; + } + return false; +} + +inline std::string ResolveSceneConfigPath(const std::string& scene_config, + const std::string& sim_config_path) { + if (scene_config.empty()) return scene_config; + + const std::filesystem::path scene_path(scene_config); + if (scene_path.is_absolute() || std::filesystem::exists(scene_path)) { + return scene_path.string(); + } + + return (std::filesystem::path(sim_config_path) / scene_path).string(); +} + +} // namespace projectairsim_ros2_cpp::bridge diff --git a/ros/projectairsim_ros2_cpp/msg/RadarReturn.msg b/ros/projectairsim_ros2_cpp/msg/RadarReturn.msg new file mode 100644 index 00000000..698f03d6 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/msg/RadarReturn.msg @@ -0,0 +1,5 @@ +float32 range +float32 azimuth +float32 elevation +float32 doppler_velocity +float32 amplitude diff --git a/ros/projectairsim_ros2_cpp/msg/RadarScan.msg b/ros/projectairsim_ros2_cpp/msg/RadarScan.msg new file mode 100644 index 00000000..d72f6631 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/msg/RadarScan.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +projectairsim_ros2_cpp/RadarReturn[] returns diff --git a/ros/projectairsim_ros2_cpp/msg/RadarTrack.msg b/ros/projectairsim_ros2_cpp/msg/RadarTrack.msg new file mode 100644 index 00000000..5fd65db1 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/msg/RadarTrack.msg @@ -0,0 +1,7 @@ +int32 id +geometry_msgs/Vector3 position +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration +float32 azimuth +float32 elevation +float32 range diff --git a/ros/projectairsim_ros2_cpp/msg/RadarTracks.msg b/ros/projectairsim_ros2_cpp/msg/RadarTracks.msg new file mode 100644 index 00000000..2512c549 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/msg/RadarTracks.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +projectairsim_ros2_cpp/RadarTrack[] tracks diff --git a/ros/projectairsim_ros2_cpp/package.xml b/ros/projectairsim_ros2_cpp/package.xml new file mode 100644 index 00000000..dd221103 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/package.xml @@ -0,0 +1,30 @@ + + + projectairsim_ros2_cpp + 0.1.0 + ROS 2 bridge for Project AirSim using the C++ client API. + + To Do + MIT + + ament_cmake + rosidl_default_generators + + geometry_msgs + nav_msgs + rclcpp_action + rclcpp + rosgraph_msgs + rosidl_runtime_cpp + sensor_msgs + std_msgs + tf2_ros + rosidl_default_runtime + rosidl_interface_packages + + ament_cmake_gtest + + + ament_cmake + + diff --git a/ros/projectairsim_ros2_cpp/src/projectairsim_ros2_cpp_node.cpp b/ros/projectairsim_ros2_cpp/src/projectairsim_ros2_cpp_node.cpp new file mode 100644 index 00000000..c62a54ae --- /dev/null +++ b/ros/projectairsim_ros2_cpp/src/projectairsim_ros2_cpp_node.cpp @@ -0,0 +1,1412 @@ +// Copyright (C) 2026 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "msgpack.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "projectairsim_ros2_cpp/action/move_on_path.hpp" +#include "projectairsim_ros2_cpp/msg/radar_scan.hpp" +#include "projectairsim_ros2_cpp/msg/radar_tracks.hpp" +#include "projectairsim_ros2_cpp/ros2_conversion_utils.hpp" +#include "projectairsim_ros2_cpp/srv/arm.hpp" +#include "projectairsim_ros2_cpp/srv/arm_group.hpp" +#include "projectairsim_ros2_cpp/srv/cancel_all_requests.hpp" +#include "projectairsim_ros2_cpp/srv/create_voxel_grid.hpp" +#include "projectairsim_ros2_cpp/srv/disarm.hpp" +#include "projectairsim_ros2_cpp/srv/disarm_group.hpp" +#include "projectairsim_ros2_cpp/srv/get_client_info.hpp" +#include "projectairsim_ros2_cpp/srv/get_clock.hpp" +#include "projectairsim_ros2_cpp/srv/get_drones.hpp" +#include "projectairsim_ros2_cpp/srv/get_origin_geo_point.hpp" +#include "projectairsim_ros2_cpp/srv/get_segmentation_id_by_name.hpp" +#include "projectairsim_ros2_cpp/srv/get_segmentation_id_map.hpp" +#include "projectairsim_ros2_cpp/srv/get_topic_info.hpp" +#include "projectairsim_ros2_cpp/srv/land.hpp" +#include "projectairsim_ros2_cpp/srv/land_group.hpp" +#include "projectairsim_ros2_cpp/srv/load_scene.hpp" +#include "projectairsim_ros2_cpp/srv/move_on_path.hpp" +#include "projectairsim_ros2_cpp/srv/move_to_position.hpp" +#include "projectairsim_ros2_cpp/srv/occupancy_grid.hpp" +#include "projectairsim_ros2_cpp/srv/publish.hpp" +#include "projectairsim_ros2_cpp/srv/raw_request.hpp" +#include "projectairsim_ros2_cpp/srv/reset.hpp" +#include "projectairsim_ros2_cpp/srv/set_object_material.hpp" +#include "projectairsim_ros2_cpp/srv/set_object_texture.hpp" +#include "projectairsim_ros2_cpp/srv/set_segmentation_id_by_name.hpp" +#include "projectairsim_ros2_cpp/srv/swap_object_texture.hpp" +#include "projectairsim_ros2_cpp/srv/takeoff.hpp" +#include "projectairsim_ros2_cpp/srv/takeoff_group.hpp" +#include "projectairsim_ros2_cpp/srv/unsubscribe.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rosgraph_msgs/msg/clock.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/fluid_pressure.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "std_msgs/msg/header.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2_ros/transform_broadcaster.h" + +namespace pasc = microsoft::projectairsim::client; +namespace mp = microsoft::projectairsim; +using nlohmann::json; +using namespace projectairsim_ros2_cpp::bridge; + +namespace { + +template +struct HasSuccessField : std::false_type {}; + +template +struct HasSuccessField().success)>> + : std::true_type {}; + +template +void SetSuccessIfPresent(T& response, bool success) { + if constexpr (HasSuccessField::value) { + response.success = success; + } else { + (void)response; + (void)success; + } +} + +std::string StatusToString(pasc::Status status) { + char buffer[256] = {0}; + pasc::GetStatusString(status, buffer); + return std::string(buffer); +} + +std_msgs::msg::Header MakeHeader(rclcpp::Node& node, + const std::string& frame_id) { + std_msgs::msg::Header header; + header.stamp = node.get_clock()->now(); + header.frame_id = frame_id; + return header; +} + +} // namespace + +class ProjectAirSimROS2CppNode final : public rclcpp::Node { + public: + using MoveOnPathAction = projectairsim_ros2_cpp::action::MoveOnPath; + using GoalHandleMoveOnPath = rclcpp_action::ServerGoalHandle; + + ProjectAirSimROS2CppNode() + : Node("projectairsim_ros2_cpp_node"), + client_(std::make_shared()), + world_(std::make_shared()) { + address_ = declare_parameter("address", "127.0.0.1"); + port_topics_ = declare_parameter("port_topics", pasc::Client::kPortTopicsDefault); + port_services_ = + declare_parameter("port_services", pasc::Client::kPortServicesDefault); + scene_config_ = declare_parameter("scene_config", ""); + sim_config_path_ = declare_parameter( + "sim_config_path", "client/python/example_user_scripts/sim_config"); + delay_after_load_sec_ = declare_parameter("delay_after_load_sec", 2.0); + projectairsim_topic_root_ = + declare_parameter("projectairsim_topic_root", "/Sim"); + ros_topic_root_ = declare_parameter("ros_topic_root", "/ProjectAirsim"); + publish_unmatched_as_json_ = + declare_parameter("publish_unmatched_as_json", false); + publish_tf_ = declare_parameter("publish_tf", true); + tf_world_frame_id_ = declare_parameter("tf_world_frame_id", "map"); + refresh_topics_period_sec_ = + declare_parameter("refresh_topics_period_sec", 0.0); + publish_clock_period_sec_ = + declare_parameter("publish_clock_period_sec", 0.02); + vehicle_name_ = declare_parameter("vehicle_name", "Drone1"); + service_root_ = declare_parameter("service_root", "/projectairsim"); + + const auto status = + client_->Connect(address_, static_cast(port_topics_), + static_cast(port_services_)); + if (status != pasc::Status::OK) { + throw std::runtime_error("Project AirSim connect failed: " + + StatusToString(status)); + } + + RCLCPP_INFO(get_logger(), "Connected to Project AirSim at %s (%d/%d)", + address_.c_str(), port_topics_, port_services_); + + if (!scene_config_.empty()) { + const auto scene_config_path = + ResolveSceneConfigPath(scene_config_, sim_config_path_); + const auto world_status = world_->Initialize( + client_, scene_config_path, sim_config_path_, + static_cast(delay_after_load_sec_)); + if (world_status != pasc::Status::OK) { + throw std::runtime_error("Project AirSim scene load failed: " + + StatusToString(world_status)); + } + RCLCPP_INFO(get_logger(), "Loaded Project AirSim scene %s from %s", + scene_config_path.c_str(), sim_config_path_.c_str()); + scene_loaded_ = true; + } else { + AdoptLoadedSceneIfAvailable(); + } + + topic_info_publisher_ = create_publisher( + "/projectairsim/topic_info", rclcpp::QoS(1).transient_local()); + clock_publisher_ = + create_publisher("/clock", rclcpp::QoS(10)); + if (publish_tf_) { + tf_broadcaster_ = std::make_unique(*this); + } + CreateServices(vehicle_name_); + CreateActionServer(vehicle_name_); + DiscoverAndSubscribeIfReady(); + StartClockTimerIfReady(); + if (refresh_topics_period_sec_ > 0.0) { + refresh_timer_ = create_wall_timer( + std::chrono::duration_cast( + std::chrono::duration(refresh_topics_period_sec_)), + std::bind(&ProjectAirSimROS2CppNode::DiscoverAndSubscribeIfReady, this)); + } + } + + ~ProjectAirSimROS2CppNode() override { + if (client_) { + client_->UnsubscribeAll(); + client_->Disconnect(); + } + } + + private: + using TopicHandler = std::function; + + bool AdoptLoadedSceneIfAvailable() { + if (scene_loaded_) return true; + + const auto status = world_->Initialize(client_); + if (status != pasc::Status::OK) { + if (!reported_waiting_for_scene_) { + RCLCPP_WARN(get_logger(), + "No Project AirSim scene is loaded yet: %s. Topic discovery " + "and /clock will start after a scene is available.", + StatusToString(status).c_str()); + reported_waiting_for_scene_ = true; + } + return false; + } + + clock_parent_topic_ = world_->GetParentTopic(); + scene_loaded_ = true; + reported_waiting_for_scene_ = false; + RCLCPP_INFO(get_logger(), "Using existing Project AirSim scene %s", + clock_parent_topic_.c_str()); + return true; + } + + void DiscoverAndSubscribeIfReady() { + if (!scene_loaded_ && !AdoptLoadedSceneIfAvailable()) return; + DiscoverAndSubscribe(); + StartClockTimerIfReady(); + } + + void DiscoverAndSubscribe() { + std::vector topics; + { + std::lock_guard lock(client_mutex_); + topics = client_->GetTopicInfo(); + } + if (!listed_projectairsim_topics_) { + RCLCPP_INFO(get_logger(), "Project AirSim reported %zu topics", topics.size()); + std_msgs::msg::String topic_info_msg; + topic_info_msg.data = json(topics).dump(); + topic_info_publisher_->publish(topic_info_msg); + for (const auto& topic : topics) { + RCLCPP_INFO(get_logger(), " Project AirSim topic: %s", topic.c_str()); + } + listed_projectairsim_topics_ = true; + } + + for (const auto& topic : topics) { + CacheClockParentTopic(topic); + + { + std::lock_guard lock(subscriptions_mutex_); + if (std::find(subscribed_topics_.begin(), subscribed_topics_.end(), topic) != + subscribed_topics_.end()) { + continue; + } + } + + auto handler = CreateHandler(topic); + if (!handler) continue; + + const auto status = client_->Subscribe( + topic.c_str(), [this](const std::string& topic_name, + const std::string& payload) { + HandleProjectAirSimMessage(topic_name, payload); + }); + if (status != pasc::Status::OK) { + RCLCPP_WARN(get_logger(), "Failed to subscribe to %s: %s", + topic.c_str(), StatusToString(status).c_str()); + continue; + } + + { + std::lock_guard lock(subscriptions_mutex_); + if (std::find(subscribed_topics_.begin(), subscribed_topics_.end(), topic) != + subscribed_topics_.end()) { + continue; + } + handlers_[topic] = std::move(*handler); + subscribed_topics_.push_back(topic); + } + RCLCPP_INFO(get_logger(), "Bridging Project AirSim topic %s -> ROS topic %s", + topic.c_str(), ToRosTopic(topic).c_str()); + } + } + + std::optional CreateHandler(const std::string& topic) { + if (EndsWith(topic, "/gps")) return CreateGpsPublisher(topic); + if (EndsWith(topic, "/actual_pose")) return CreatePosePublisher(topic); + if (EndsWith(topic, "/imu") || EndsWith(topic, "/imu_kinematics")) { + return CreateImuPublisher(topic); + } + if (EndsWith(topic, "/barometer")) return CreateBarometerPublisher(topic); + if (EndsWith(topic, "/magnetometer")) return CreateMagnetometerPublisher(topic); + if (EndsWith(topic, "/lidar")) return CreateLidarPublisher(topic); + if (EndsWith(topic, "/radar_detections")) return CreateRadarScanPublisher(topic); + if (EndsWith(topic, "/radar_tracks")) return CreateRadarTracksPublisher(topic); + if (EndsWith(topic, "_camera_info")) return CreateCameraInfoPublisher(topic); + if (EndsWith(topic, "_camera") || EndsWith(topic, "/scene_camera") || + EndsWith(topic, "/depth_camera") || EndsWith(topic, "/depth_planar_camera") || + EndsWith(topic, "/depth_vis_camera") || + EndsWith(topic, "/disparity_normalized_camera") || + EndsWith(topic, "/segmentation_camera") || + EndsWith(topic, "/surface_normals_camera")) { + return CreateImagePublisher(topic); + } + if (publish_unmatched_as_json_) return CreateJsonPublisher(topic); + return std::nullopt; + } + + std::string ToRosTopic(const std::string& projectairsim_topic) const { + if (StartsWithPathPrefix(projectairsim_topic, projectairsim_topic_root_)) { + return ros_topic_root_ + + projectairsim_topic.substr(projectairsim_topic_root_.size()); + } + return projectairsim_topic; + } + + void MaybeBroadcastTransform(const std::string& parent_frame_id, + const std::string& child_frame_id, + const json& position, + const json& orientation) { + if (!tf_broadcaster_ || child_frame_id.empty()) return; + + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = get_clock()->now(); + transform.header.frame_id = parent_frame_id; + transform.child_frame_id = child_frame_id; + transform.transform.translation = ToRosVector3(position); + transform.transform.rotation = ToRosQuaternion(orientation); + tf_broadcaster_->sendTransform(transform); + } + + TopicHandler CreateGpsPublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = + create_publisher(ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = TopicToFrameId(topic), topic]( + const std::string&, const json& msg) { + sensor_msgs::msg::NavSatFix ros_msg; + ros_msg.header = MakeHeader(*this, frame_id); + ros_msg.status.status = + NumberOr(msg, "fix_type") >= 2.0 + ? sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX + : sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + ros_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + ros_msg.latitude = NumberOr(msg, "latitude"); + ros_msg.longitude = NumberOr(msg, "longitude"); + ros_msg.altitude = NumberOr(msg, "altitude"); + ros_msg.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + publisher->publish(ros_msg); + }; + } + + TopicHandler CreatePosePublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = + create_publisher(ros_topic, rclcpp::QoS(10)); + return [this, publisher, child_frame_id = TopicToFrameId(topic)]( + const std::string&, const json& msg) { + geometry_msgs::msg::PoseStamped ros_msg; + ros_msg.header = MakeHeader(*this, tf_world_frame_id_); + ros_msg.pose.position = ToRosPoint(msg.value("position", json::object())); + ros_msg.pose.orientation = + ToRosQuaternion(msg.value("orientation", json::object())); + MaybeBroadcastTransform(tf_world_frame_id_, child_frame_id, + msg.value("position", json::object()), + msg.value("orientation", json::object())); + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateImuPublisher(const std::string& topic) { + const auto ros_topic = ReplaceSuffix(ToRosTopic(topic), "/imu_kinematics", "/imu"); + auto publisher = + create_publisher(ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = TopicToFrameId(topic), topic]( + const std::string&, const json& msg) { + sensor_msgs::msg::Imu ros_msg; + ros_msg.header = MakeHeader(*this, frame_id); + ros_msg.orientation = + ToRosQuaternion(msg.value("orientation", json::object())); + ros_msg.angular_velocity = + ToRosVector3(msg.value("angular_velocity", json::array())); + ros_msg.linear_acceleration = + ToRosVector3(msg.value("linear_acceleration", json::array())); + ros_msg.orientation_covariance.fill(0.0); + ros_msg.angular_velocity_covariance.fill(0.0); + ros_msg.linear_acceleration_covariance.fill(0.0); + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateBarometerPublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = create_publisher( + ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = TopicToFrameId(topic)]( + const std::string&, const json& msg) { + sensor_msgs::msg::FluidPressure ros_msg; + ros_msg.header = MakeHeader(*this, frame_id); + ros_msg.fluid_pressure = NumberOr(msg, "pressure"); + ros_msg.variance = 0.0; + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateMagnetometerPublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = create_publisher( + ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = TopicToFrameId(topic)]( + const std::string&, const json& msg) { + sensor_msgs::msg::MagneticField ros_msg; + ros_msg.header = MakeHeader(*this, frame_id); + ros_msg.magnetic_field = + ToRosVector3(msg.value("magnetic_field_body", json::array())); + ros_msg.magnetic_field_covariance.fill(0.0); + const auto covariance = + ArrayNumbers(msg.value("magnetic_field_covariance", json::array())); + for (size_t i = 0; i < std::min(covariance.size(), 9); ++i) { + ros_msg.magnetic_field_covariance[i] = covariance[i]; + } + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateLidarPublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = create_publisher( + ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = TopicToFrameId(topic)]( + const std::string&, const json& msg) { + const auto points = ArrayNumbers(msg.value("point_cloud", json::array())); + sensor_msgs::msg::PointCloud2 cloud; + cloud.header = MakeHeader(*this, msg.value("frame_id", frame_id)); + cloud.height = 1; + cloud.width = static_cast(points.size() / 3); + cloud.is_bigendian = false; + cloud.is_dense = true; + + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(cloud.width); + + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + for (size_t i = 0; i + 2 < points.size(); i += 3, ++iter_x, ++iter_y, ++iter_z) { + *iter_x = static_cast(points[i]); + *iter_y = static_cast(-points[i + 1]); + *iter_z = static_cast(-points[i + 2]); + } + publisher->publish(cloud); + }; + } + + TopicHandler CreateRadarScanPublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = create_publisher( + ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = TopicToFrameId(topic)]( + const std::string&, const json& msg) { + projectairsim_ros2_cpp::msg::RadarScan ros_msg; + ros_msg.header = MakeHeader(*this, frame_id); + AppendRadarReturnsFromJson(msg.value("radar_detections", json::array()), + &ros_msg); + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateRadarTracksPublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = create_publisher( + ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = TopicToFrameId(topic)]( + const std::string&, const json& msg) { + projectairsim_ros2_cpp::msg::RadarTracks ros_msg; + ros_msg.header = MakeHeader(*this, frame_id); + AppendRadarTracksFromJson(msg.value("radar_tracks", json::array()), &ros_msg); + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateCameraInfoPublisher(const std::string& topic) { + const auto image_ros_topic = ToRosTopic(CameraImageTopicFromInfoTopic(topic)); + const auto ros_topic = CameraInfoRosTopic(image_ros_topic); + auto publisher = create_publisher( + ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = CameraFrameId(topic)]( + const std::string&, const json& msg) { + sensor_msgs::msg::CameraInfo ros_msg; + ros_msg.header = MakeHeader(*this, frame_id); + PopulateCameraInfoFromJson(msg, &ros_msg); + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateImagePublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = + create_publisher(ros_topic, rclcpp::SensorDataQoS()); + return [this, publisher, frame_id = CameraFrameId(topic), topic]( + const std::string&, const json& msg) { + sensor_msgs::msg::Image ros_msg; + ros_msg.header = MakeHeader(*this, msg.value("frame_id", frame_id)); + if (!PopulateImagePayloadFromJson(msg, &ros_msg)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Unsupported image encoding on %s: %s", + topic.c_str(), + msg.value("encoding", std::string()).c_str()); + return; + } + if (msg.contains("position") && msg.contains("orientation")) { + MaybeBroadcastTransform(tf_world_frame_id_, ros_msg.header.frame_id, + msg.value("position", json::object()), + msg.value("orientation", json::object())); + } else if (msg.contains("pos_x") && msg.contains("rot_w")) { + MaybeBroadcastTransform(tf_world_frame_id_, ros_msg.header.frame_id, + json{{"x", NumberOr(msg, "pos_x")}, + {"y", NumberOr(msg, "pos_y")}, + {"z", NumberOr(msg, "pos_z")}}, + json{{"x", NumberOr(msg, "rot_x")}, + {"y", NumberOr(msg, "rot_y")}, + {"z", NumberOr(msg, "rot_z")}, + {"w", NumberOr(msg, "rot_w", 1.0)}}); + } + publisher->publish(ros_msg); + }; + } + + TopicHandler CreateJsonPublisher(const std::string& topic) { + const auto ros_topic = ToRosTopic(topic); + auto publisher = create_publisher(ros_topic, rclcpp::QoS(10)); + return [publisher](const std::string&, const json& msg) { + std_msgs::msg::String ros_msg; + ros_msg.data = msg.dump(); + publisher->publish(ros_msg); + }; + } + + bool RequestBool(const std::string& method, const json& params = json::object()) { + std::lock_guard lock(client_mutex_); + pasc::Message message_response; + const auto status = client_->Request(method, params, &message_response); + if (status != pasc::Status::OK) { + RCLCPP_ERROR(get_logger(), "%s request failed: %s", method.c_str(), + StatusToString(status).c_str()); + return false; + } + + mp::ResponseMessage response; + response.Deserialize(message_response); + if (response.GetErrorCode() != 0) { + RCLCPP_ERROR(get_logger(), "%s rejected by server: %s", method.c_str(), + response.GetResult().dump().c_str()); + return false; + } + + const auto result = response.GetResult(); + if (result.is_boolean()) return result.get(); + if (result.is_object() && result.contains("success") && result["success"].is_boolean()) { + return result["success"].get(); + } + return true; + } + + bool RequestJson(const std::string& method, const std::string& json_parameters, + int32_t* error_code, std::string* result_json, + std::string* raw_response, std::string* status_text) { + json params = json::object(); + if (!json_parameters.empty()) { + try { + params = json::parse(json_parameters); + } catch (const json::parse_error& error) { + *error_code = -1; + *status_text = std::string("Invalid JSON parameters: ") + error.what(); + *result_json = json({{"error", *status_text}}).dump(); + raw_response->clear(); + return false; + } + } + + std::lock_guard lock(client_mutex_); + pasc::Message message_response; + const auto status = client_->Request(method, params, &message_response); + *status_text = StatusToString(status); + *raw_response = static_cast(message_response); + if (status != pasc::Status::OK) { + *error_code = static_cast(status); + *result_json = json({{"error", *status_text}}).dump(); + RCLCPP_ERROR(get_logger(), "%s request failed: %s", method.c_str(), + status_text->c_str()); + return false; + } + + mp::ResponseMessage response; + response.Deserialize(message_response); + *error_code = response.GetErrorCode(); + *result_json = response.GetResult().dump(); + return *error_code == 0; + } + + bool RequestJsonObject(const std::string& method, const json& params, + json* result, std::string* status_text) { + std::lock_guard lock(client_mutex_); + pasc::Message message_response; + const auto status = client_->Request(method, params, &message_response); + *status_text = StatusToString(status); + if (status != pasc::Status::OK) { + return false; + } + + mp::ResponseMessage response; + response.Deserialize(message_response); + *result = response.GetResult(); + if (response.GetErrorCode() != 0) { + *status_text = response.GetResult().dump(); + return false; + } + return true; + } + + bool RequestWorldJson(const std::string& method, const json& params, + json* result, std::string* status_text) { + const auto parent_topic = ResolveClockParentTopic(); + if (parent_topic.empty()) { + *status_text = "Project AirSim scene parent topic is not resolved"; + return false; + } + return RequestJsonObject(parent_topic + "/" + method, params, result, status_text); + } + + bool RequestWorldBool(const std::string& method, const json& params, + std::string* status_text) { + json result; + if (!RequestWorldJson(method, params, &result, status_text)) { + return false; + } + + if (!result.is_boolean()) { + *status_text = "Expected boolean result from " + method + ": " + result.dump(); + return false; + } + + if (!result.get()) { + *status_text = method + " returned false"; + return false; + } + + return true; + } + + bool RequestVehicleJson(const std::string& vehicle_name, const std::string& method, + const json& params, json* result, + std::string* status_text) { + const auto parent_topic = ResolveClockParentTopic(); + if (parent_topic.empty()) { + *status_text = "Project AirSim scene parent topic is not resolved"; + return false; + } + const auto resolved_vehicle_name = + vehicle_name.empty() ? vehicle_name_ : vehicle_name; + return RequestJsonObject(parent_topic + "/robots/" + resolved_vehicle_name + "/" + + method, + params, result, status_text); + } + + bool PublishJson(const std::string& topic, const std::string& json_message, + std::string* status_text) { + json message; + try { + message = json::parse(json_message); + } catch (const json::parse_error& error) { + *status_text = std::string("Invalid JSON message: ") + error.what(); + return false; + } + + std::lock_guard lock(client_mutex_); + const auto status = client_->Publish(topic.c_str(), message); + *status_text = StatusToString(status); + return status == pasc::Status::OK; + } + + bool UnsubscribeTopics(const std::vector& topics, bool all, + std::string* status_text) { + std::lock_guard lock(client_mutex_); + const auto status = all ? client_->UnsubscribeAll() + : client_->Unsubscribe(topics); + *status_text = StatusToString(status); + if (status == pasc::Status::OK) { + std::lock_guard subscriptions_lock(subscriptions_mutex_); + if (all) { + subscribed_topics_.clear(); + handlers_.clear(); + } else { + for (const auto& topic : topics) { + subscribed_topics_.erase( + std::remove(subscribed_topics_.begin(), subscribed_topics_.end(), topic), + subscribed_topics_.end()); + handlers_.erase(topic); + } + } + } + return status == pasc::Status::OK; + } + + bool GetClientInfo(std::string* client_version, std::string* nng_version, + std::string* build_commit_hash) { + *client_version = pasc::Client::GetVersion(); + *nng_version = pasc::Client::GetNNGVersion(); + std::lock_guard lock(client_mutex_); + const auto status = client_->GetBuildCommitHash(build_commit_hash); + if (status != pasc::Status::OK) { + RCLCPP_WARN(get_logger(), "GetBuildCommitHash failed: %s", + StatusToString(status).c_str()); + } + return status == pasc::Status::OK; + } + + void CacheClockParentTopic(const std::string& topic) { + if (!clock_parent_topic_.empty()) return; + const auto parent_topic = + ParentTopicFromProjectAirSimTopic(topic, projectairsim_topic_root_); + if (parent_topic.empty()) return; + + clock_parent_topic_ = parent_topic; + RCLCPP_INFO(get_logger(), "Resolved Project AirSim clock parent topic: %s", + clock_parent_topic_.c_str()); + } + + std::string ResolveClockParentTopic() { + if (!clock_parent_topic_.empty()) return clock_parent_topic_; + + const std::string world_parent_topic = world_->GetParentTopic(); + if (IsSceneParentTopic(world_parent_topic, projectairsim_topic_root_)) { + clock_parent_topic_ = world_parent_topic; + return clock_parent_topic_; + } + return clock_parent_topic_; + } + + bool RequestSimTime(std::int64_t* nanosec) { + if (!scene_loaded_) return false; + const auto parent_topic = ResolveClockParentTopic(); + if (!parent_topic.empty()) { + const auto method = parent_topic + "/GetSimTime"; + pasc::Message message_response; + pasc::Status status = pasc::Status::OK; + { + std::lock_guard lock(client_mutex_); + status = client_->Request(method, json::object(), &message_response); + } + if (status == pasc::Status::OK) { + mp::ResponseMessage response; + response.Deserialize(message_response); + if (response.GetErrorCode() == 0 && + ExtractSimTimeNanos(response.GetResult(), nanosec)) { + return true; + } + + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "%s returned an invalid sim time response: error=%d result=%s", + method.c_str(), response.GetErrorCode(), + response.GetResult().dump().c_str()); + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "%s request failed: %s", method.c_str(), + StatusToString(status).c_str()); + } + } + + const auto status = world_->GetSimTime(nanosec); + if (status != pasc::Status::OK) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "GetSimTime fallback failed: %s", + StatusToString(status).c_str()); + return false; + } + return true; + } + + std::shared_ptr GetDrone(const std::string& vehicle_name) { + std::lock_guard lock(drones_mutex_); + auto it = drones_.find(vehicle_name); + if (it != drones_.end()) return it->second; + + auto drone = std::make_shared(); + const auto status = drone->Initialize(client_, world_, vehicle_name); + if (status != pasc::Status::OK) { + RCLCPP_ERROR(get_logger(), "Failed to initialize drone %s: %s", + vehicle_name.c_str(), StatusToString(status).c_str()); + return nullptr; + } + + drones_[vehicle_name] = drone; + return drone; + } + + static pasc::Drone::YawControlMode YawModeFromDriveTrain(int drive_train_type) { + return drive_train_type == 1 ? pasc::Drone::YawControlMode::ForwardOnly + : pasc::Drone::YawControlMode::MaxDegreeOfFreedom; + } + + static pasc::VecVector3 PathFromRos( + const std::vector& path) { + pasc::VecVector3 result; + result.reserve(path.size()); + for (const auto& pose : path) { + result.push_back({static_cast(pose.pose.position.x), + static_cast(pose.pose.position.y), + static_cast(pose.pose.position.z)}); + } + return result; + } + + static int CellsFromSize(double size, double resolution) { + if (resolution <= 0.0 || size <= 0.0) return 0; + return static_cast(std::floor(size / resolution)); + } + + static pasc::Pose PoseFromPosition(double x, double y, double z) { + pasc::Pose pose; + pose.position = {static_cast(x), static_cast(y), + static_cast(z)}; + pose.orientation = {1.0f, 0.0f, 0.0f, 0.0f}; + return pose; + } + + bool EnableAndArm(const std::string& vehicle_name) { + auto drone = GetDrone(vehicle_name); + if (!drone) return false; + bool state = false; + auto status = drone->EnableAPIControl(&state); + if (status != pasc::Status::OK) return false; + status = drone->Arm(&state); + return status == pasc::Status::OK; + } + + bool Takeoff(const std::string& vehicle_name, bool wait_on_last_task) { + auto drone = GetDrone(vehicle_name); + if (!drone || !EnableAndArm(vehicle_name)) return false; + auto result = drone->TakeoffAsync(10.0f); + return !wait_on_last_task || result.Wait() == pasc::Status::OK; + } + + bool Land(const std::string& vehicle_name, bool wait_on_last_task) { + auto drone = GetDrone(vehicle_name); + if (!drone) return false; + auto result = drone->LandAsync(); + return !wait_on_last_task || result.Wait() == pasc::Status::OK; + } + + bool Arm(const std::string& vehicle_name, bool /*wait_on_last_task*/) { + return EnableAndArm(vehicle_name); + } + + bool Disarm(const std::string& vehicle_name, bool /*wait_on_last_task*/) { + auto drone = GetDrone(vehicle_name); + if (!drone) return false; + bool state = false; + const auto disarm_status = drone->Disarm(&state); + const auto disable_status = drone->DisableAPIControl(&state); + if (disarm_status != pasc::Status::OK) { + RCLCPP_ERROR(get_logger(), "Disarm failed: %s", + StatusToString(disarm_status).c_str()); + } + if (disable_status != pasc::Status::OK) { + RCLCPP_ERROR(get_logger(), "DisableAPIControl after disarm failed: %s", + StatusToString(disable_status).c_str()); + } + return disarm_status == pasc::Status::OK && disable_status == pasc::Status::OK; + } + + bool MoveToPosition(const std::string& vehicle_name, double x, double y, double z, + double velocity, float timeout_sec, int drive_train_type, + bool yaw_is_rate, float yaw, float lookahead, + float adaptive_lookahead, bool wait_on_last_task) { + auto drone = GetDrone(vehicle_name); + if (!drone) return false; + auto result = drone->MoveToPositionAsync( + static_cast(x), static_cast(y), static_cast(z), + static_cast(velocity), timeout_sec, YawModeFromDriveTrain(drive_train_type), + yaw_is_rate, yaw, lookahead, adaptive_lookahead); + return !wait_on_last_task || result.Wait() == pasc::Status::OK; + } + + bool MoveOnPath(const std::string& vehicle_name, + const std::vector& path, + double velocity, float timeout_sec, int drive_train_type, + bool yaw_is_rate, float yaw, float lookahead, + float adaptive_lookahead, bool wait_on_last_task) { + auto drone = GetDrone(vehicle_name); + if (!drone) return false; + auto result = drone->MoveOnPathAsync( + PathFromRos(path), static_cast(velocity), timeout_sec, + YawModeFromDriveTrain(drive_train_type), yaw_is_rate, yaw, lookahead, + adaptive_lookahead); + return !wait_on_last_task || result.Wait() == pasc::Status::OK; + } + + bool CreateVoxelGrid(double x, double y, double z, int64_t x_size, + int64_t y_size, int64_t z_size, double resolution, + const std::string& output_file, + pasc::BoolArray* voxel_grid = nullptr) { + if (x_size <= 0 || y_size <= 0 || z_size <= 0 || resolution <= 0.0) { + RCLCPP_ERROR(get_logger(), + "CreateVoxelGrid requires positive sizes and resolution"); + return false; + } + + pasc::BoolArray local_voxel_grid; + auto& result_grid = voxel_grid != nullptr ? *voxel_grid : local_voxel_grid; + const bool write_file = !output_file.empty(); + const auto status = world_->CreateVoxelGrid( + PoseFromPosition(x, y, z), static_cast(x_size), + static_cast(y_size), static_cast(z_size), + static_cast(resolution), &result_grid, {}, write_file, output_file); + if (status != pasc::Status::OK) { + RCLCPP_ERROR(get_logger(), "CreateVoxelGrid failed: %s", + StatusToString(status).c_str()); + return false; + } + return true; + } + + nav_msgs::msg::OccupancyGrid OccupancyGridFromVoxels( + const pasc::BoolArray& voxel_grid, double center_x, double center_y, + double center_z, int64_t x_size, int64_t y_size, int64_t z_size, + double resolution) { + (void)center_z; + nav_msgs::msg::OccupancyGrid grid; + grid.header = MakeHeader(*this, "map"); + grid.info.resolution = static_cast(resolution); + grid.info.width = static_cast(CellsFromSize(x_size, resolution)); + grid.info.height = static_cast(CellsFromSize(y_size, resolution)); + grid.info.origin.position.x = center_x - 0.5 * static_cast(x_size); + grid.info.origin.position.y = center_y - 0.5 * static_cast(y_size); + grid.info.origin.position.z = 0.0; + grid.info.origin.orientation.w = 1.0; + + const int nx = static_cast(grid.info.width); + const int ny = static_cast(grid.info.height); + const int nz = CellsFromSize(z_size, resolution); + grid.data.assign(static_cast(nx * ny), -1); + + if (nx <= 0 || ny <= 0 || nz <= 0) return grid; + const size_t expected_voxels = static_cast(nx) * + static_cast(ny) * + static_cast(nz); + if (voxel_grid.Cf() < expected_voxels) { + RCLCPP_ERROR(get_logger(), "Voxel grid size %zu is smaller than expected %zu", + voxel_grid.Cf(), expected_voxels); + return grid; + } + + for (int y = 0; y < ny; ++y) { + for (int x = 0; x < nx; ++x) { + bool occupied = false; + for (int z = 0; z < nz; ++z) { + const size_t voxel_index = + static_cast(x + nx * (z + nz * y)); + occupied = occupied || voxel_grid[voxel_index]; + } + grid.data[static_cast(x + nx * y)] = occupied ? 100 : 0; + } + } + return grid; + } + + bool GetClock(std::int64_t* nanosec) { + return RequestSimTime(nanosec); + } + + void PublishClock() { + if (!scene_loaded_) return; + std::int64_t nanosec = 0; + if (!RequestSimTime(&nanosec)) { + return; + } + + rosgraph_msgs::msg::Clock clock_msg; + const auto sec = nanosec / 1000000000LL; + auto nsec = nanosec % 1000000000LL; + if (nsec < 0) nsec += 1000000000LL; + clock_msg.clock.sec = static_cast(sec); + clock_msg.clock.nanosec = static_cast(nsec); + clock_publisher_->publish(clock_msg); + } + + void StartClockTimerIfReady() { + if (!scene_loaded_ || publish_clock_period_sec_ <= 0.0 || clock_timer_) { + return; + } + clock_timer_ = create_wall_timer( + std::chrono::duration_cast( + std::chrono::duration(publish_clock_period_sec_)), + std::bind(&ProjectAirSimROS2CppNode::PublishClock, this)); + } + + bool GetOriginGeoPoint(double* latitude, double* longitude, double* altitude) { + const auto& config = world_->GetConfiguration(); + if (!config.contains("home-geo-point") || + !config["home-geo-point"].is_object()) { + RCLCPP_ERROR(get_logger(), "Scene configuration has no home-geo-point"); + return false; + } + const auto& home_geo_point = config["home-geo-point"]; + *latitude = NumberOr(home_geo_point, "latitude"); + *longitude = NumberOr(home_geo_point, "longitude"); + *altitude = NumberOr(home_geo_point, "altitude"); + return true; + } + + bool LoadSceneRuntime(const std::string& scene_file, bool is_primary_client) { + if (!is_primary_client) { + return AdoptLoadedSceneIfAvailable(); + } + const auto scene_config_path = ResolveSceneConfigPath(scene_file, sim_config_path_); + const auto status = world_->Initialize( + client_, scene_config_path, sim_config_path_, + static_cast(delay_after_load_sec_)); + if (status != pasc::Status::OK) { + RCLCPP_ERROR(get_logger(), "Load scene failed: %s", StatusToString(status).c_str()); + return false; + } + + { + std::lock_guard lock(drones_mutex_); + drones_.clear(); + } + { + std::lock_guard lock(subscriptions_mutex_); + subscribed_topics_.clear(); + handlers_.clear(); + } + clock_parent_topic_ = world_->GetParentTopic(); + scene_loaded_ = true; + reported_waiting_for_scene_ = false; + listed_projectairsim_topics_ = false; + DiscoverAndSubscribe(); + StartClockTimerIfReady(); + return true; + } + + template + void CreateService(const std::string& name, Fn&& fn) { + services_.push_back(create_service( + name, + [this, name, fn = std::forward(fn)]( + const std::shared_ptr request, + std::shared_ptr response) { + try { + fn(request, response); + } catch (const std::exception& error) { + SetSuccessIfPresent(*response, false); + RCLCPP_ERROR(get_logger(), "%s failed: %s", name.c_str(), error.what()); + } + })); + } + + template + void CreateGroupService(const std::string& name, Fn&& fn) { + CreateService( + name, [fn = std::forward(fn)](const auto request, auto response) { + response->success = true; + for (const auto& vehicle_name : request->vehicle_names) { + response->success = + fn(vehicle_name, request->wait_on_last_task) && response->success; + } + }); + } + + void CreateServices(const std::string& vehicle_name) { + const std::string prefix = service_root_ + "/" + vehicle_name; + + CreateService( + service_root_ + "/request", [this](const auto request, auto response) { + response->success = + RequestJson(request->method, request->json_parameters, + &response->error_code, &response->result_json, + &response->raw_response, &response->status); + }); + CreateService( + service_root_ + "/get_client_info", + [this](const auto request, auto response) { + (void)request; + response->success = + GetClientInfo(&response->client_version, &response->nng_version, + &response->build_commit_hash); + }); + CreateService( + service_root_ + "/get_topic_info", [this](const auto request, auto response) { + (void)request; + std::lock_guard lock(client_mutex_); + response->topics = client_->GetTopicInfo(); + response->topics_json = json(response->topics).dump(); + response->success = true; + }); + CreateService( + service_root_ + "/get_drones", [this](const auto request, auto response) { + (void)request; + const auto& drones = world_->GetDrones(); + response->drones.assign(drones.begin(), drones.end()); + response->success = true; + }); + CreateService( + service_root_ + "/publish", [this](const auto request, auto response) { + response->success = + PublishJson(request->topic, request->json_message, &response->status); + }); + CreateService( + service_root_ + "/unsubscribe", [this](const auto request, auto response) { + response->success = + UnsubscribeTopics(request->topics, request->all, &response->status); + }); + CreateService( + service_root_ + "/cancel_all_requests", + [this](const auto request, auto response) { + (void)request; + client_->CancelAllRequests(); + response->success = true; + }); + + CreateService( + prefix + "/takeoff", [this, vehicle_name](const auto request, auto response) { + response->success = Takeoff(vehicle_name, request->wait_on_last_task); + }); + CreateService( + prefix + "/land", [this, vehicle_name](const auto request, auto response) { + response->success = Land(vehicle_name, request->wait_on_last_task); + }); + CreateService( + prefix + "/arm", [this, vehicle_name](const auto request, auto response) { + response->success = Arm(vehicle_name, request->wait_on_last_task); + }); + CreateService( + prefix + "/disarm", [this, vehicle_name](const auto request, auto response) { + response->success = Disarm(vehicle_name, request->wait_on_last_task); + }); + CreateService( + prefix + "/move_to_position", + [this, vehicle_name](const auto request, auto response) { + response->success = MoveToPosition( + vehicle_name, request->x, request->y, request->z, request->velocity, + request->timeout_sec, request->drive_train_type, request->yaw_is_rate, + request->yaw, request->lookahead, request->adaptive_lookahead, + request->wait_on_last_task); + }); + CreateService( + prefix + "/move_on_path", [this, vehicle_name](const auto request, auto response) { + response->success = MoveOnPath( + vehicle_name, request->path, request->velocity, request->timeout_sec, + request->drive_train_type, request->yaw_is_rate, request->yaw, + request->lookahead, request->adaptive_lookahead, + request->wait_on_last_task); + }); + + CreateGroupService( + service_root_ + "/takeoff_group", + [this](const std::string& name, bool wait) { return Takeoff(name, wait); }); + CreateGroupService( + service_root_ + "/land_group", + [this](const std::string& name, bool wait) { return Land(name, wait); }); + CreateGroupService( + service_root_ + "/arm_group", + [this](const std::string& name, bool wait) { return Arm(name, wait); }); + CreateGroupService( + service_root_ + "/disarm_group", + [this](const std::string& name, bool wait) { return Disarm(name, wait); }); + CreateService( + service_root_ + "/create_voxel_grid", + [this](const auto request, auto response) { + (void)request->n_z_resolution; + response->success = + CreateVoxelGrid(request->position_x, request->position_y, + request->position_z, request->ncells_x, + request->ncells_y, request->ncells_z, + request->resolution, request->output_file); + }); + CreateService( + service_root_ + "/occupancy_grid", + [this](const auto request, auto response) { + (void)request->n_z_resolution; + pasc::BoolArray voxel_grid; + response->success = + CreateVoxelGrid(request->position_x, request->position_y, + request->position_z, request->ncells_x, + request->ncells_y, request->ncells_z, request->res, + "", &voxel_grid); + if (response->success) { + response->map = OccupancyGridFromVoxels( + voxel_grid, request->position_x, request->position_y, + request->position_z, request->ncells_x, request->ncells_y, + request->ncells_z, request->res); + } + }); + CreateService( + service_root_ + "/get_clock", [this](const auto request, auto response) { + (void)request; + std::int64_t nanosec = 0; + if (GetClock(&nanosec)) response->nanosec = nanosec; + }); + CreateService( + service_root_ + "/get_origin_geo_point", + [this](const auto request, auto response) { + (void)request; + double latitude = 0.0; + double longitude = 0.0; + double altitude = 0.0; + if (GetOriginGeoPoint(&latitude, &longitude, &altitude)) { + response->latitude = latitude; + response->longitude = longitude; + response->altitude = altitude; + } + }); + CreateService( + service_root_ + "/set_segmentation_id_by_name", + [this](const auto request, auto response) { + response->success = RequestWorldBool( + "SetSegmentationIDByName", + json{{"mesh_name", request->mesh_name}, + {"segmentation_id", request->segmentation_id}, + {"is_name_regex", request->is_name_regex}, + {"use_owner_name", request->use_owner_name}}, + &response->status); + }); + CreateService( + service_root_ + "/get_segmentation_id_by_name", + [this](const auto request, auto response) { + json result; + response->success = RequestWorldJson( + "GetSegmentationIDByName", + json{{"mesh_name", request->mesh_name}, + {"use_owner_name", request->use_owner_name}}, + &result, &response->status); + if (response->success && result.is_number_integer()) { + response->segmentation_id = result.get(); + } + }); + CreateService( + service_root_ + "/get_segmentation_id_map", + [this](const auto request, auto response) { + (void)request; + json result; + response->success = RequestWorldJson( + "GetSegmentationIDMap", json::object(), &result, &response->status); + if (response->success) response->segmentation_map_json = result.dump(); + }); + CreateService( + service_root_ + "/set_object_material", [this](const auto request, auto response) { + response->success = RequestWorldBool( + "SetObjectMaterial", + json{{"object_name", request->object_name}, + {"material_asset_path", request->material_asset_path}}, + &response->status); + }); + CreateService( + service_root_ + "/set_object_texture", [this](const auto request, auto response) { + std::string method; + json params = {{"object_name", request->object_name}}; + if (request->source == + projectairsim_ros2_cpp::srv::SetObjectTexture::Request::SOURCE_URL) { + method = "SetObjectTextureFromUrl"; + params["url"] = request->texture; + } else if ( + request->source == + projectairsim_ros2_cpp::srv::SetObjectTexture::Request::SOURCE_FILE) { + method = "SetObjectTextureFromFile"; + params["texture_file_path"] = request->texture; + } else if ( + request->source == + projectairsim_ros2_cpp::srv::SetObjectTexture::Request:: + SOURCE_PACKAGED_ASSET) { + method = "SetObjectTextureFromPackagedAsset"; + params["texture_asset_path"] = request->texture; + } else { + response->success = false; + response->status = "Invalid texture source"; + return; + } + response->success = RequestWorldBool(method, params, &response->status); + }); + CreateService( + service_root_ + "/swap_object_texture", [this](const auto request, auto response) { + response->success = RequestWorldBool( + "SwapObjectTexture", + json{{"tag", request->tag}, {"tex_id", request->tex_id}}, + &response->status); + }); + CreateService( + service_root_ + "/reset", [this](const auto request, auto response) { + (void)request; + response->success = RequestBool("/Sim/Reset"); + }); + CreateService( + service_root_ + "/load_scene", [this](const auto request, auto response) { + response->success = + LoadSceneRuntime(request->scene_file, request->is_primary_client); + }); + } + + void CreateActionServer(const std::string& vehicle_name) { + action_server_ = rclcpp_action::create_server( + this, service_root_ + "/" + vehicle_name + "/move_on_path", + [](const rclcpp_action::GoalUUID&, + std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [](const std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this, vehicle_name](const std::shared_ptr goal_handle) { + std::thread([this, vehicle_name, goal_handle]() { + ExecuteMoveOnPath(vehicle_name, goal_handle); + }).detach(); + }); + } + + void ExecuteMoveOnPath(const std::string& vehicle_name, + const std::shared_ptr goal_handle) { + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + feedback->current_point_on_path = "submitted"; + goal_handle->publish_feedback(feedback); + + auto result = std::make_shared(); + result->success = + MoveOnPath(vehicle_name, goal->path, goal->velocity, goal->timeout_sec, + goal->drive_train_type, goal->yaw_is_rate, goal->yaw, + goal->lookahead, goal->adaptive_lookahead, + goal->wait_on_last_task); + + if (result->success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } + + void HandleProjectAirSimMessage(const std::string& topic_name, + const std::string& payload) { + try { + const auto msg = UnpackProjectAirSimMessage(payload); + TopicHandler handler; + { + std::lock_guard lock(subscriptions_mutex_); + auto handler_it = handlers_.find(topic_name); + if (handler_it != handlers_.end()) handler = handler_it->second; + } + if (handler) handler(topic_name, msg); + } catch (const std::exception& ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Failed to convert Project AirSim topic %s: %s", + topic_name.c_str(), ex.what()); + } + } + + std::shared_ptr client_; + std::shared_ptr world_; + std::mutex client_mutex_; + std::mutex drones_mutex_; + std::mutex subscriptions_mutex_; + std::string address_; + int port_topics_; + int port_services_; + std::string scene_config_; + std::string sim_config_path_; + double delay_after_load_sec_; + std::string projectairsim_topic_root_; + std::string ros_topic_root_; + std::string vehicle_name_; + std::string service_root_; + std::string clock_parent_topic_; + bool publish_unmatched_as_json_; + bool publish_tf_; + std::string tf_world_frame_id_; + double refresh_topics_period_sec_; + double publish_clock_period_sec_; + bool listed_projectairsim_topics_ = false; + bool scene_loaded_ = false; + bool reported_waiting_for_scene_ = false; + rclcpp::TimerBase::SharedPtr refresh_timer_; + rclcpp::TimerBase::SharedPtr clock_timer_; + rclcpp::Publisher::SharedPtr topic_info_publisher_; + rclcpp::Publisher::SharedPtr clock_publisher_; + std::unique_ptr tf_broadcaster_; + std::vector services_; + rclcpp_action::Server::SharedPtr action_server_; + std::vector subscribed_topics_; + std::map handlers_; + std::map> drones_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + try { + rclcpp::spin(std::make_shared()); + } catch (const std::exception& ex) { + RCLCPP_FATAL(rclcpp::get_logger("projectairsim_ros2_cpp_node"), "%s", ex.what()); + rclcpp::shutdown(); + return 1; + } + rclcpp::shutdown(); + return 0; +} diff --git a/ros/projectairsim_ros2_cpp/srv/Arm.srv b/ros/projectairsim_ros2_cpp/srv/Arm.srv new file mode 100644 index 00000000..632fbf4b --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/Arm.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/ArmGroup.srv b/ros/projectairsim_ros2_cpp/srv/ArmGroup.srv new file mode 100644 index 00000000..54103ad3 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/ArmGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/CancelAllRequests.srv b/ros/projectairsim_ros2_cpp/srv/CancelAllRequests.srv new file mode 100644 index 00000000..410e0f9d --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/CancelAllRequests.srv @@ -0,0 +1,2 @@ +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/CreateVoxelGrid.srv b/ros/projectairsim_ros2_cpp/srv/CreateVoxelGrid.srv new file mode 100644 index 00000000..2893642d --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/CreateVoxelGrid.srv @@ -0,0 +1,11 @@ +float64 position_x +float64 position_y +float64 position_z +int64 ncells_x +int64 ncells_y +int64 ncells_z +float64 resolution +int64 n_z_resolution +string output_file +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/Disarm.srv b/ros/projectairsim_ros2_cpp/srv/Disarm.srv new file mode 100644 index 00000000..632fbf4b --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/Disarm.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/DisarmGroup.srv b/ros/projectairsim_ros2_cpp/srv/DisarmGroup.srv new file mode 100644 index 00000000..54103ad3 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/DisarmGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/GetClientInfo.srv b/ros/projectairsim_ros2_cpp/srv/GetClientInfo.srv new file mode 100644 index 00000000..20ff33a9 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/GetClientInfo.srv @@ -0,0 +1,5 @@ +--- +string client_version +string nng_version +string build_commit_hash +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/GetClock.srv b/ros/projectairsim_ros2_cpp/srv/GetClock.srv new file mode 100644 index 00000000..2ad1af7c --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/GetClock.srv @@ -0,0 +1,2 @@ +--- +int64 nanosec diff --git a/ros/projectairsim_ros2_cpp/srv/GetDrones.srv b/ros/projectairsim_ros2_cpp/srv/GetDrones.srv new file mode 100644 index 00000000..ab00feb0 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/GetDrones.srv @@ -0,0 +1,3 @@ +--- +string[] drones +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/GetOriginGeoPoint.srv b/ros/projectairsim_ros2_cpp/srv/GetOriginGeoPoint.srv new file mode 100644 index 00000000..7e7ea03e --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/GetOriginGeoPoint.srv @@ -0,0 +1,4 @@ +--- +float64 latitude +float64 longitude +float64 altitude diff --git a/ros/projectairsim_ros2_cpp/srv/GetSegmentationIDByName.srv b/ros/projectairsim_ros2_cpp/srv/GetSegmentationIDByName.srv new file mode 100644 index 00000000..9cf0e830 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/GetSegmentationIDByName.srv @@ -0,0 +1,6 @@ +string mesh_name +bool use_owner_name +--- +bool success +int32 segmentation_id +string status diff --git a/ros/projectairsim_ros2_cpp/srv/GetSegmentationIDMap.srv b/ros/projectairsim_ros2_cpp/srv/GetSegmentationIDMap.srv new file mode 100644 index 00000000..2bd8e8c1 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/GetSegmentationIDMap.srv @@ -0,0 +1,4 @@ +--- +bool success +string segmentation_map_json +string status diff --git a/ros/projectairsim_ros2_cpp/srv/GetTopicInfo.srv b/ros/projectairsim_ros2_cpp/srv/GetTopicInfo.srv new file mode 100644 index 00000000..7498d2b0 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/GetTopicInfo.srv @@ -0,0 +1,4 @@ +--- +string[] topics +string topics_json +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/Land.srv b/ros/projectairsim_ros2_cpp/srv/Land.srv new file mode 100644 index 00000000..632fbf4b --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/Land.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/LandGroup.srv b/ros/projectairsim_ros2_cpp/srv/LandGroup.srv new file mode 100644 index 00000000..54103ad3 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/LandGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/LoadScene.srv b/ros/projectairsim_ros2_cpp/srv/LoadScene.srv new file mode 100644 index 00000000..361bfa5a --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/LoadScene.srv @@ -0,0 +1,4 @@ +string scene_file +bool is_primary_client +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/MoveOnPath.srv b/ros/projectairsim_ros2_cpp/srv/MoveOnPath.srv new file mode 100644 index 00000000..a6a29068 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/MoveOnPath.srv @@ -0,0 +1,11 @@ +geometry_msgs/PoseStamped[] path +float64 velocity +float32 timeout_sec +int32 drive_train_type +bool yaw_is_rate +float32 yaw +float32 lookahead +float32 adaptive_lookahead +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/MoveToPosition.srv b/ros/projectairsim_ros2_cpp/srv/MoveToPosition.srv new file mode 100644 index 00000000..c64d7ae6 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/MoveToPosition.srv @@ -0,0 +1,13 @@ +float64 x +float64 y +float64 z +float64 velocity +float32 timeout_sec +int32 drive_train_type +bool yaw_is_rate +float32 yaw +float32 lookahead +float32 adaptive_lookahead +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/OccupancyGrid.srv b/ros/projectairsim_ros2_cpp/srv/OccupancyGrid.srv new file mode 100644 index 00000000..a51c7c4f --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/OccupancyGrid.srv @@ -0,0 +1,12 @@ +# Position should be in global NED +float64 position_x +float64 position_y +float64 position_z +int64 ncells_x +int64 ncells_y +int64 ncells_z +float64 res +int64 n_z_resolution +--- +nav_msgs/OccupancyGrid map +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/Publish.srv b/ros/projectairsim_ros2_cpp/srv/Publish.srv new file mode 100644 index 00000000..1fccae6f --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/Publish.srv @@ -0,0 +1,5 @@ +string topic +string json_message +--- +bool success +string status diff --git a/ros/projectairsim_ros2_cpp/srv/RawRequest.srv b/ros/projectairsim_ros2_cpp/srv/RawRequest.srv new file mode 100644 index 00000000..f19a3cdb --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/RawRequest.srv @@ -0,0 +1,8 @@ +string method +string json_parameters +--- +bool success +string status +int32 error_code +string result_json +string raw_response diff --git a/ros/projectairsim_ros2_cpp/srv/Reset.srv b/ros/projectairsim_ros2_cpp/srv/Reset.srv new file mode 100644 index 00000000..632fbf4b --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/Reset.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/SetObjectMaterial.srv b/ros/projectairsim_ros2_cpp/srv/SetObjectMaterial.srv new file mode 100644 index 00000000..8c133a62 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/SetObjectMaterial.srv @@ -0,0 +1,5 @@ +string object_name +string material_asset_path +--- +bool success +string status diff --git a/ros/projectairsim_ros2_cpp/srv/SetObjectTexture.srv b/ros/projectairsim_ros2_cpp/srv/SetObjectTexture.srv new file mode 100644 index 00000000..cee24704 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/SetObjectTexture.srv @@ -0,0 +1,9 @@ +uint8 SOURCE_URL=0 +uint8 SOURCE_FILE=1 +uint8 SOURCE_PACKAGED_ASSET=2 +string object_name +uint8 source +string texture +--- +bool success +string status diff --git a/ros/projectairsim_ros2_cpp/srv/SetSegmentationIDByName.srv b/ros/projectairsim_ros2_cpp/srv/SetSegmentationIDByName.srv new file mode 100644 index 00000000..22e156fd --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/SetSegmentationIDByName.srv @@ -0,0 +1,7 @@ +string mesh_name +int32 segmentation_id +bool is_name_regex +bool use_owner_name +--- +bool success +string status diff --git a/ros/projectairsim_ros2_cpp/srv/SwapObjectTexture.srv b/ros/projectairsim_ros2_cpp/srv/SwapObjectTexture.srv new file mode 100644 index 00000000..8a3ca127 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/SwapObjectTexture.srv @@ -0,0 +1,5 @@ +string tag +int32 tex_id +--- +bool success +string status diff --git a/ros/projectairsim_ros2_cpp/srv/Takeoff.srv b/ros/projectairsim_ros2_cpp/srv/Takeoff.srv new file mode 100644 index 00000000..632fbf4b --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/Takeoff.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/TakeoffGroup.srv b/ros/projectairsim_ros2_cpp/srv/TakeoffGroup.srv new file mode 100644 index 00000000..54103ad3 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/TakeoffGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool wait_on_last_task +--- +bool success diff --git a/ros/projectairsim_ros2_cpp/srv/Unsubscribe.srv b/ros/projectairsim_ros2_cpp/srv/Unsubscribe.srv new file mode 100644 index 00000000..29af23dc --- /dev/null +++ b/ros/projectairsim_ros2_cpp/srv/Unsubscribe.srv @@ -0,0 +1,5 @@ +string[] topics +bool all +--- +bool success +string status diff --git a/ros/projectairsim_ros2_cpp/test/ros2_bridge_surface_regression_test.cpp b/ros/projectairsim_ros2_cpp/test/ros2_bridge_surface_regression_test.cpp new file mode 100644 index 00000000..a5387284 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/test/ros2_bridge_surface_regression_test.cpp @@ -0,0 +1,119 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace { + +std::filesystem::path PackageRoot() { + return std::filesystem::path(__FILE__).parent_path().parent_path(); +} + +std::string ReadFile(const std::filesystem::path& path) { + std::ifstream file(path); + if (!file.is_open()) { + ADD_FAILURE() << "Unable to open " << path; + return {}; + } + + std::ostringstream contents; + contents << file.rdbuf(); + return contents.str(); +} + +void ExpectContains(const std::string& haystack, const std::string& needle) { + EXPECT_NE(haystack.find(needle), std::string::npos) + << "Missing expected text: " << needle; +} + +TEST(Ros2BridgeSurfaceRegression, InterfacesAreDeclaredAndRegistered) { + const auto package_root = PackageRoot(); + const auto cmake = ReadFile(package_root / "CMakeLists.txt"); + + const std::vector expected_interfaces = { + "msg/RadarReturn.msg", + "msg/RadarScan.msg", + "msg/RadarTrack.msg", + "msg/RadarTracks.msg", + "srv/GetClock.srv", + "srv/GetSegmentationIDByName.srv", + "srv/GetSegmentationIDMap.srv", + "srv/SetObjectMaterial.srv", + "srv/SetObjectTexture.srv", + "srv/SetSegmentationIDByName.srv", + "srv/SwapObjectTexture.srv", + }; + + for (const auto& interface : expected_interfaces) { + EXPECT_TRUE(std::filesystem::is_regular_file(package_root / interface)) + << "Missing " << interface; + ExpectContains(cmake, "\"" + interface + "\""); + } +} + +TEST(Ros2BridgeSurfaceRegression, BridgeSourceKeepsConversionSurface) { + const auto source = + ReadFile(PackageRoot() / "src" / "projectairsim_ros2_cpp_node.cpp"); + + const std::vector expected_tokens = { + "create_publisher(\"/clock\"", + "std::make_unique", + "EndsWith(topic, \"/radar_detections\")", + "CreateRadarScanPublisher(topic)", + "EndsWith(topic, \"/radar_tracks\")", + "CreateRadarTracksPublisher(topic)", + "EndsWith(topic, \"_camera_info\")", + "CreateCameraInfoPublisher(topic)", + "CameraInfoRosTopic(image_ros_topic)", + "AdoptLoadedSceneIfAvailable()", + "world_->Initialize(client_)", + "if (!is_primary_client)", + "subscriptions_mutex_", + "TopicHandler handler;", + "\"SetSegmentationIDByName\"", + "\"GetSegmentationIDByName\"", + "\"GetSegmentationIDMap\"", + "\"SetObjectMaterial\"", + "\"SetObjectTextureFromUrl\"", + "\"SetObjectTextureFromFile\"", + "\"SetObjectTextureFromPackagedAsset\"", + "\"SwapObjectTexture\"", + }; + + for (const auto& token : expected_tokens) { + ExpectContains(source, token); + } + + const std::vector expected_service_names = { + "/projectairsim/request", + "/projectairsim/get_client_info", + "/projectairsim/get_topic_info", + "/projectairsim/get_drones", + "/projectairsim/publish", + "/projectairsim/unsubscribe", + "/projectairsim/cancel_all_requests", + "/projectairsim/get_clock", + "/projectairsim/set_segmentation_id_by_name", + "/projectairsim/get_segmentation_id_by_name", + "/projectairsim/get_segmentation_id_map", + "/projectairsim/set_object_material", + "/projectairsim/set_object_texture", + "/projectairsim/swap_object_texture", + }; + + for (const auto& service : expected_service_names) { + const auto service_suffix = service.substr(std::string("/projectairsim").size()); + const auto direct = source.find(service) != std::string::npos; + const auto rooted = + source.find("service_root_ + \"" + service_suffix + "\"") != + std::string::npos; + EXPECT_TRUE(direct || rooted) << "Missing service " << service; + } +} + +} // namespace diff --git a/ros/projectairsim_ros2_cpp/test/ros2_conversion_utils_test.cpp b/ros/projectairsim_ros2_cpp/test/ros2_conversion_utils_test.cpp new file mode 100644 index 00000000..59f5796f --- /dev/null +++ b/ros/projectairsim_ros2_cpp/test/ros2_conversion_utils_test.cpp @@ -0,0 +1,168 @@ +#include + +#include +#include +#include +#include + +#include "msgpack.hpp" +#include "projectairsim_ros2_cpp/msg/radar_scan.hpp" +#include "projectairsim_ros2_cpp/msg/radar_tracks.hpp" +#include "projectairsim_ros2_cpp/ros2_conversion_utils.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" + +namespace { + +namespace bridge = projectairsim_ros2_cpp::bridge; +using bridge::json; + +TEST(Ros2ConversionUtils, TopicAndFrameHelpersMatchBridgeConventions) { + EXPECT_TRUE(bridge::EndsWith("/Sim/Scene/robots/Drone1/imu", "/imu")); + EXPECT_TRUE(bridge::StartsWithPathPrefix("/Sim/Scene/robots", "/Sim")); + EXPECT_FALSE(bridge::StartsWithPathPrefix("/Simulator/Scene", "/Sim")); + + EXPECT_EQ(bridge::TopicToFrameId("/Sim/Scene/robots/Drone1/imu"), "Drone1"); + EXPECT_EQ(bridge::TopicToFrameId("/Sim/Scene/robots/Drone1/imu_kinematics"), + "Drone1"); + EXPECT_EQ(bridge::CameraFrameId("/Sim/Scene/robots/Drone1/Down/scene_camera_info"), + "Sim_Scene_robots_Drone1_Down_scene_camera"); + EXPECT_EQ(bridge::CameraImageTopicFromInfoTopic( + "/Sim/Scene/robots/Drone1/Down/scene_camera_info"), + "/Sim/Scene/robots/Drone1/Down/scene_camera"); + EXPECT_EQ(bridge::CameraInfoRosTopic( + "/ProjectAirsim/Scene/robots/Drone1/Down/scene_camera"), + "/ProjectAirsim/Scene/robots/Drone1/Down/scene_camera/camera_info"); + EXPECT_EQ(bridge::ParentTopicFromProjectAirSimTopic( + "/Sim/Scene/robots/Drone1/imu", "/Sim"), + "/Sim/Scene"); + EXPECT_EQ(bridge::ParentTopicFromProjectAirSimTopic("/$topics", "/"), ""); + EXPECT_TRUE(bridge::IsSceneParentTopic("/Sim/Scene", "/Sim")); + EXPECT_FALSE(bridge::IsSceneParentTopic("/Sim", "/Sim")); + EXPECT_FALSE(bridge::IsSceneParentTopic("/$topics", "/")); +} + +TEST(Ros2ConversionUtils, SimTimeExtractionAcceptsSupportedPayloadShapes) { + std::int64_t nanos = 0; + + ASSERT_TRUE(bridge::ExtractSimTimeNanos(1234, &nanos)); + EXPECT_EQ(nanos, 1234); + + ASSERT_TRUE(bridge::ExtractSimTimeNanos(json{{"sim_time_nanos", 5678}}, &nanos)); + EXPECT_EQ(nanos, 5678); + + ASSERT_TRUE(bridge::ExtractSimTimeNanos(json{{"sec", 2}, {"nanosec", 9}}, &nanos)); + EXPECT_EQ(nanos, 2000000009LL); + + EXPECT_FALSE(bridge::ExtractSimTimeNanos(json{{"time", "bad"}}, &nanos)); +} + +TEST(Ros2ConversionUtils, CoordinateConversionsApplyRosAxisConvention) { + const auto vector_from_array = bridge::ToRosVector3(json::array({1.0, 2.0, 3.0})); + EXPECT_DOUBLE_EQ(vector_from_array.x, 1.0); + EXPECT_DOUBLE_EQ(vector_from_array.y, -2.0); + EXPECT_DOUBLE_EQ(vector_from_array.z, -3.0); + + const auto point = bridge::ToRosPoint(json{{"x", 4.0}, {"y", 5.0}, {"z", -6.0}}); + EXPECT_DOUBLE_EQ(point.x, 4.0); + EXPECT_DOUBLE_EQ(point.y, -5.0); + EXPECT_DOUBLE_EQ(point.z, 6.0); + + const auto quaternion = + bridge::ToRosQuaternion(json{{"x", 0.1}, {"y", 0.2}, {"z", 0.3}, {"w", 0.4}}); + EXPECT_DOUBLE_EQ(quaternion.x, 0.1); + EXPECT_DOUBLE_EQ(quaternion.y, -0.2); + EXPECT_DOUBLE_EQ(quaternion.z, -0.3); + EXPECT_DOUBLE_EQ(quaternion.w, 0.4); +} + +TEST(Ros2ConversionUtils, CameraInfoAndImagePayloadsAreConverted) { + sensor_msgs::msg::CameraInfo camera_info; + bridge::PopulateCameraInfoFromJson( + json{{"width", 640}, + {"height", 480}, + {"distortion_model", "plumb_bob"}, + {"distortion_params", json::array({0.1, 0.2})}, + {"intrinsic_camera_matrix", json::array({1, 0, 320, 0, 1, 240, 0, 0, 1})}, + {"rectification_matrix", json::array({1, 0, 0, 0, 1, 0, 0, 0, 1})}, + {"projection_matrix", + json::array({1, 0, 320, 0, 0, 1, 240, 0, 0, 0, 1, 0})}}, + &camera_info); + + EXPECT_EQ(camera_info.width, 640u); + EXPECT_EQ(camera_info.height, 480u); + EXPECT_EQ(camera_info.distortion_model, "plumb_bob"); + EXPECT_EQ(camera_info.d.size(), 2u); + EXPECT_DOUBLE_EQ(camera_info.k[2], 320.0); + EXPECT_DOUBLE_EQ(camera_info.p[10], 1.0); + + sensor_msgs::msg::Image bgr_image; + ASSERT_TRUE(bridge::PopulateImagePayloadFromJson( + json{{"height", 2}, {"width", 3}, {"big_endian", 0}, {"encoding", "BGR"}, + {"data", std::string("abcdefghijklmnopqr")}}, + &bgr_image)); + EXPECT_EQ(bgr_image.encoding, "bgr8"); + EXPECT_EQ(bgr_image.step, 9u); + EXPECT_EQ(bgr_image.data.size(), 18u); + + sensor_msgs::msg::Image depth_image; + ASSERT_TRUE(bridge::PopulateImagePayloadFromJson( + json{{"height", 1}, {"width", 2}, {"encoding", "16UC1"}, + {"data", std::string("\x01\x00\x02\x00", 4)}}, + &depth_image)); + EXPECT_EQ(depth_image.encoding, "mono16"); + EXPECT_EQ(depth_image.step, 4u); + + sensor_msgs::msg::Image unsupported; + EXPECT_FALSE(bridge::PopulateImagePayloadFromJson( + json{{"height", 1}, {"width", 1}, {"encoding", "RGB"}}, &unsupported)); +} + +TEST(Ros2ConversionUtils, RadarPayloadsAreConverted) { + projectairsim_ros2_cpp::msg::RadarScan scan; + bridge::AppendRadarReturnsFromJson( + json::array({json{{"range", 10.0}, + {"azimuth", 0.25}, + {"elevation", -0.1}, + {"velocity", 2.5}, + {"rcs_sqm", 3.0}}}), + &scan); + + ASSERT_EQ(scan.returns.size(), 1u); + EXPECT_FLOAT_EQ(scan.returns[0].range, 10.0F); + EXPECT_FLOAT_EQ(scan.returns[0].azimuth, 0.25F); + EXPECT_FLOAT_EQ(scan.returns[0].doppler_velocity, 2.5F); + EXPECT_FLOAT_EQ(scan.returns[0].amplitude, + bridge::RadarAmplitudeFromRangeAndRcs(10.0, 3.0)); + + projectairsim_ros2_cpp::msg::RadarTracks tracks; + bridge::AppendRadarTracksFromJson( + json::array({json{{"id", 3}, + {"position_est", json::array({1.0, 2.0, 3.0})}, + {"velocity_est", json::array({4.0, 5.0, 6.0})}, + {"accel_est", json::array({7.0, 8.0, 9.0})}, + {"azimuth_est", 0.7}, + {"elevation_est", 0.8}, + {"range_est", 15.0}}}), + &tracks); + + ASSERT_EQ(tracks.tracks.size(), 1u); + EXPECT_EQ(tracks.tracks[0].id, 3); + EXPECT_DOUBLE_EQ(tracks.tracks[0].position.y, -2.0); + EXPECT_DOUBLE_EQ(tracks.tracks[0].velocity.z, -6.0); + EXPECT_FLOAT_EQ(tracks.tracks[0].range, 15.0F); +} + +TEST(Ros2ConversionUtils, MsgpackPayloadUnpacksToJson) { + msgpack::sbuffer buffer; + const std::map payload{{"answer", 42}}; + msgpack::pack(buffer, payload); + + const auto unpacked = + bridge::UnpackProjectAirSimMessage(std::string(buffer.data(), buffer.size())); + + ASSERT_TRUE(unpacked.is_object()); + EXPECT_EQ(unpacked["answer"].get(), 42); +} + +} // namespace diff --git a/ros/projectairsim_ros2_cpp/test/ros2_generated_interfaces_regression_test.cpp b/ros/projectairsim_ros2_cpp/test/ros2_generated_interfaces_regression_test.cpp new file mode 100644 index 00000000..823e1b61 --- /dev/null +++ b/ros/projectairsim_ros2_cpp/test/ros2_generated_interfaces_regression_test.cpp @@ -0,0 +1,133 @@ +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "projectairsim_ros2_cpp/action/move_on_path.hpp" +#include "projectairsim_ros2_cpp/msg/radar_return.hpp" +#include "projectairsim_ros2_cpp/msg/radar_scan.hpp" +#include "projectairsim_ros2_cpp/msg/radar_track.hpp" +#include "projectairsim_ros2_cpp/msg/radar_tracks.hpp" +#include "projectairsim_ros2_cpp/srv/get_clock.hpp" +#include "projectairsim_ros2_cpp/srv/get_segmentation_id_by_name.hpp" +#include "projectairsim_ros2_cpp/srv/get_segmentation_id_map.hpp" +#include "projectairsim_ros2_cpp/srv/raw_request.hpp" +#include "projectairsim_ros2_cpp/srv/set_object_material.hpp" +#include "projectairsim_ros2_cpp/srv/set_object_texture.hpp" +#include "projectairsim_ros2_cpp/srv/set_segmentation_id_by_name.hpp" +#include "projectairsim_ros2_cpp/srv/swap_object_texture.hpp" + +namespace { + +TEST(Ros2GeneratedInterfacesRegression, RadarMessageInterfacesMatchExpectedFields) { + projectairsim_ros2_cpp::msg::RadarReturn radar_return; + radar_return.range = 10.0F; + radar_return.azimuth = 0.25F; + radar_return.elevation = -0.1F; + radar_return.doppler_velocity = 2.5F; + radar_return.amplitude = 12.0F; + + projectairsim_ros2_cpp::msg::RadarScan scan; + scan.returns.push_back(radar_return); + + projectairsim_ros2_cpp::msg::RadarTrack track; + track.id = 7; + track.position.x = 1.0; + track.velocity.y = 2.0; + track.acceleration.z = -3.0; + track.azimuth = 0.1F; + track.elevation = 0.2F; + track.range = 30.0F; + + projectairsim_ros2_cpp::msg::RadarTracks tracks; + tracks.tracks.push_back(track); + + EXPECT_FLOAT_EQ(scan.returns[0].range, 10.0F); + EXPECT_EQ(tracks.tracks[0].id, 7); + EXPECT_DOUBLE_EQ(tracks.tracks[0].acceleration.z, -3.0); +} + +TEST(Ros2GeneratedInterfacesRegression, GenericAndClockServicesAreUsable) { + projectairsim_ros2_cpp::srv::RawRequest::Request raw_request; + raw_request.method = "/Sim/GetBuildCommitHash"; + raw_request.json_parameters = "{}"; + + projectairsim_ros2_cpp::srv::RawRequest::Response raw_response; + raw_response.success = true; + raw_response.error_code = 0; + raw_response.result_json = "\"abc123\""; + + projectairsim_ros2_cpp::srv::GetClock::Response clock_response; + clock_response.nanosec = 123456789; + + EXPECT_EQ(raw_request.method, "/Sim/GetBuildCommitHash"); + EXPECT_TRUE(raw_response.success); + EXPECT_EQ(clock_response.nanosec, 123456789); +} + +TEST(Ros2GeneratedInterfacesRegression, + SegmentationMaterialAndTextureServicesMatchBridgeContract) { + projectairsim_ros2_cpp::srv::SetSegmentationIDByName::Request set_seg; + set_seg.mesh_name = "Car.*"; + set_seg.segmentation_id = 42; + set_seg.is_name_regex = true; + set_seg.use_owner_name = false; + + projectairsim_ros2_cpp::srv::GetSegmentationIDByName::Response get_seg; + get_seg.success = true; + get_seg.segmentation_id = 42; + + projectairsim_ros2_cpp::srv::GetSegmentationIDMap::Response seg_map; + seg_map.success = true; + seg_map.segmentation_map_json = "{}"; + + projectairsim_ros2_cpp::srv::SetObjectMaterial::Request material; + material.object_name = "Cube"; + material.material_asset_path = "/Game/Materials/M_Test"; + + projectairsim_ros2_cpp::srv::SetObjectTexture::Request texture; + texture.object_name = "Cube"; + texture.source = + projectairsim_ros2_cpp::srv::SetObjectTexture::Request::SOURCE_URL; + texture.texture = "https://example.invalid/texture.png"; + + projectairsim_ros2_cpp::srv::SwapObjectTexture::Request swap; + swap.tag = "target"; + swap.tex_id = 2; + + EXPECT_TRUE(set_seg.is_name_regex); + EXPECT_EQ(get_seg.segmentation_id, 42); + EXPECT_EQ(seg_map.segmentation_map_json, "{}"); + EXPECT_EQ(material.material_asset_path, "/Game/Materials/M_Test"); + EXPECT_EQ(texture.source, + projectairsim_ros2_cpp::srv::SetObjectTexture::Request::SOURCE_URL); + EXPECT_EQ(swap.tex_id, 2); +} + +TEST(Ros2GeneratedInterfacesRegression, MoveOnPathActionIsGenerated) { + projectairsim_ros2_cpp::action::MoveOnPath::Goal goal; + geometry_msgs::msg::PoseStamped waypoint; + waypoint.pose.position.x = 5.0; + waypoint.pose.position.y = 5.0; + waypoint.pose.position.z = -3.0; + waypoint.pose.orientation.w = 1.0; + goal.path.push_back(waypoint); + goal.velocity = 2.0; + goal.timeout_sec = 20.0F; + goal.lookahead = -1.0F; + goal.adaptive_lookahead = 1.0F; + goal.drive_train_type = 0; + goal.yaw_is_rate = true; + goal.yaw = 0.0F; + goal.wait_on_last_task = true; + + projectairsim_ros2_cpp::action::MoveOnPath::Feedback feedback; + feedback.current_point_on_path = "submitted"; + + projectairsim_ros2_cpp::action::MoveOnPath::Result result; + result.success = true; + + EXPECT_EQ(goal.path.size(), 1u); + EXPECT_EQ(feedback.current_point_on_path, "submitted"); + EXPECT_TRUE(result.success); +} + +} // namespace diff --git a/ros/projectairsim_ros2_cpp/test/ros2_interfaces_test.cpp b/ros/projectairsim_ros2_cpp/test/ros2_interfaces_test.cpp new file mode 100644 index 00000000..d9b0866d --- /dev/null +++ b/ros/projectairsim_ros2_cpp/test/ros2_interfaces_test.cpp @@ -0,0 +1,421 @@ +#include + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "projectairsim_ros2_cpp/action/move_on_path.hpp" +#include "projectairsim_ros2_cpp/msg/radar_scan.hpp" +#include "projectairsim_ros2_cpp/msg/radar_tracks.hpp" +#include "projectairsim_ros2_cpp/srv/arm.hpp" +#include "projectairsim_ros2_cpp/srv/arm_group.hpp" +#include "projectairsim_ros2_cpp/srv/cancel_all_requests.hpp" +#include "projectairsim_ros2_cpp/srv/create_voxel_grid.hpp" +#include "projectairsim_ros2_cpp/srv/disarm.hpp" +#include "projectairsim_ros2_cpp/srv/disarm_group.hpp" +#include "projectairsim_ros2_cpp/srv/get_client_info.hpp" +#include "projectairsim_ros2_cpp/srv/get_clock.hpp" +#include "projectairsim_ros2_cpp/srv/get_drones.hpp" +#include "projectairsim_ros2_cpp/srv/get_origin_geo_point.hpp" +#include "projectairsim_ros2_cpp/srv/get_segmentation_id_by_name.hpp" +#include "projectairsim_ros2_cpp/srv/get_segmentation_id_map.hpp" +#include "projectairsim_ros2_cpp/srv/get_topic_info.hpp" +#include "projectairsim_ros2_cpp/srv/land.hpp" +#include "projectairsim_ros2_cpp/srv/land_group.hpp" +#include "projectairsim_ros2_cpp/srv/load_scene.hpp" +#include "projectairsim_ros2_cpp/srv/move_on_path.hpp" +#include "projectairsim_ros2_cpp/srv/move_to_position.hpp" +#include "projectairsim_ros2_cpp/srv/occupancy_grid.hpp" +#include "projectairsim_ros2_cpp/srv/publish.hpp" +#include "projectairsim_ros2_cpp/srv/raw_request.hpp" +#include "projectairsim_ros2_cpp/srv/reset.hpp" +#include "projectairsim_ros2_cpp/srv/set_object_material.hpp" +#include "projectairsim_ros2_cpp/srv/set_object_texture.hpp" +#include "projectairsim_ros2_cpp/srv/set_segmentation_id_by_name.hpp" +#include "projectairsim_ros2_cpp/srv/swap_object_texture.hpp" +#include "projectairsim_ros2_cpp/srv/takeoff.hpp" +#include "projectairsim_ros2_cpp/srv/takeoff_group.hpp" +#include "projectairsim_ros2_cpp/srv/unsubscribe.hpp" +#include "rosgraph_msgs/msg/clock.hpp" +#include "rosidl_runtime_cpp/traits.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/fluid_pressure.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +namespace { + +template +std::string RosTypeName() { + return rosidl_generator_traits::name(); +} + +TEST(Ros2Contracts, StandardTypedTopicMessageContracts) { + EXPECT_EQ(RosTypeName(), "std_msgs/msg/String"); + EXPECT_EQ(RosTypeName(), + "geometry_msgs/msg/PoseStamped"); + EXPECT_EQ(RosTypeName(), + "sensor_msgs/msg/NavSatFix"); + EXPECT_EQ(RosTypeName(), "sensor_msgs/msg/Imu"); + EXPECT_EQ(RosTypeName(), + "sensor_msgs/msg/FluidPressure"); + EXPECT_EQ(RosTypeName(), + "sensor_msgs/msg/MagneticField"); + EXPECT_EQ(RosTypeName(), + "sensor_msgs/msg/PointCloud2"); + EXPECT_EQ(RosTypeName(), "sensor_msgs/msg/Image"); + EXPECT_EQ(RosTypeName(), + "sensor_msgs/msg/CameraInfo"); + EXPECT_EQ(RosTypeName(), + "projectairsim_ros2_cpp/msg/RadarScan"); + EXPECT_EQ(RosTypeName(), + "projectairsim_ros2_cpp/msg/RadarTracks"); + EXPECT_EQ(RosTypeName(), + "rosgraph_msgs/msg/Clock"); +} + +TEST(Ros2Contracts, BridgeTopicNamesMatchExpectedSurface) { + const std::vector required_topics = { + "/projectairsim/topic_info", + "/clock", + "/ProjectAirsim//robots//actual_pose", + "/ProjectAirsim//robots//sensors//gps", + "/ProjectAirsim//robots//sensors//imu", + "/ProjectAirsim//robots//sensors//barometer", + "/ProjectAirsim//robots//sensors//magnetometer", + "/ProjectAirsim//robots//sensors//lidar", + "/ProjectAirsim//robots//sensors//radar_detections", + "/ProjectAirsim//robots//sensors//radar_tracks", + "/ProjectAirsim//robots//sensors///camera_info", + }; + + EXPECT_EQ(required_topics.size(), 11u); + EXPECT_EQ(required_topics.front(), "/projectairsim/topic_info"); + EXPECT_EQ(required_topics[1], "/clock"); +} + +TEST(Ros2Contracts, SingleVehicleServiceContracts) { + using projectairsim_ros2_cpp::srv::Arm; + using projectairsim_ros2_cpp::srv::Disarm; + using projectairsim_ros2_cpp::srv::Land; + using projectairsim_ros2_cpp::srv::MoveOnPath; + using projectairsim_ros2_cpp::srv::MoveToPosition; + using projectairsim_ros2_cpp::srv::Takeoff; + + Takeoff::Request takeoff_request; + takeoff_request.wait_on_last_task = true; + Takeoff::Response takeoff_response; + takeoff_response.success = true; + EXPECT_TRUE(takeoff_request.wait_on_last_task); + EXPECT_TRUE(takeoff_response.success); + + Land::Request land_request; + land_request.wait_on_last_task = true; + Land::Response land_response; + land_response.success = true; + EXPECT_TRUE(land_request.wait_on_last_task); + EXPECT_TRUE(land_response.success); + + Arm::Request arm_request; + arm_request.wait_on_last_task = true; + Arm::Response arm_response; + arm_response.success = true; + EXPECT_TRUE(arm_request.wait_on_last_task); + EXPECT_TRUE(arm_response.success); + + Disarm::Request disarm_request; + disarm_request.wait_on_last_task = true; + Disarm::Response disarm_response; + disarm_response.success = true; + EXPECT_TRUE(disarm_request.wait_on_last_task); + EXPECT_TRUE(disarm_response.success); + + MoveToPosition::Request move_to_position_request; + move_to_position_request.x = 5.0; + move_to_position_request.y = 0.0; + move_to_position_request.z = -3.0; + move_to_position_request.velocity = 2.0; + move_to_position_request.timeout_sec = 10.0F; + move_to_position_request.drive_train_type = 0; + move_to_position_request.yaw_is_rate = true; + move_to_position_request.yaw = 0.0F; + move_to_position_request.lookahead = -1.0F; + move_to_position_request.adaptive_lookahead = 1.0F; + move_to_position_request.wait_on_last_task = true; + EXPECT_DOUBLE_EQ(move_to_position_request.x, 5.0); + EXPECT_FLOAT_EQ(move_to_position_request.timeout_sec, 10.0F); + + MoveOnPath::Request move_on_path_request; + geometry_msgs::msg::PoseStamped waypoint; + waypoint.pose.position.x = 5.0; + waypoint.pose.position.y = 0.0; + waypoint.pose.position.z = -3.0; + waypoint.pose.orientation.w = 1.0; + move_on_path_request.path.push_back(waypoint); + move_on_path_request.velocity = 2.0; + move_on_path_request.timeout_sec = 20.0F; + move_on_path_request.wait_on_last_task = true; + EXPECT_EQ(move_on_path_request.path.size(), 1u); + EXPECT_DOUBLE_EQ(move_on_path_request.path[0].pose.position.x, 5.0); +} + +TEST(Ros2Contracts, GroupServiceContracts) { + using projectairsim_ros2_cpp::srv::ArmGroup; + using projectairsim_ros2_cpp::srv::DisarmGroup; + using projectairsim_ros2_cpp::srv::LandGroup; + using projectairsim_ros2_cpp::srv::TakeoffGroup; + + TakeoffGroup::Request takeoff_request; + takeoff_request.vehicle_names = {"Drone1", "Drone2"}; + takeoff_request.wait_on_last_task = true; + EXPECT_EQ(takeoff_request.vehicle_names.size(), 2u); + + LandGroup::Request land_request; + land_request.vehicle_names = {"Drone1", "Drone2"}; + land_request.wait_on_last_task = true; + EXPECT_EQ(land_request.vehicle_names[1], "Drone2"); + + ArmGroup::Request arm_request; + arm_request.vehicle_names = {"Drone1"}; + arm_request.wait_on_last_task = true; + EXPECT_EQ(arm_request.vehicle_names[0], "Drone1"); + + DisarmGroup::Request disarm_request; + disarm_request.vehicle_names = {"Drone1"}; + disarm_request.wait_on_last_task = true; + EXPECT_EQ(disarm_request.vehicle_names[0], "Drone1"); +} + +TEST(Ros2Contracts, WorldAndSceneServiceContracts) { + using projectairsim_ros2_cpp::srv::CreateVoxelGrid; + using projectairsim_ros2_cpp::srv::GetClock; + using projectairsim_ros2_cpp::srv::GetOriginGeoPoint; + using projectairsim_ros2_cpp::srv::LoadScene; + using projectairsim_ros2_cpp::srv::OccupancyGrid; + using projectairsim_ros2_cpp::srv::Reset; + + LoadScene::Request load_scene_request; + load_scene_request.scene_file = "scene_drone_sensors.jsonc"; + load_scene_request.is_primary_client = true; + EXPECT_EQ(load_scene_request.scene_file, "scene_drone_sensors.jsonc"); + + Reset::Request reset_request; + reset_request.wait_on_last_task = true; + EXPECT_TRUE(reset_request.wait_on_last_task); + + CreateVoxelGrid::Request voxel_request; + voxel_request.position_x = 0.0; + voxel_request.position_y = 0.0; + voxel_request.position_z = -4.0; + voxel_request.ncells_x = 20; + voxel_request.ncells_y = 20; + voxel_request.ncells_z = 10; + voxel_request.resolution = 1.0; + voxel_request.n_z_resolution = 10; + voxel_request.output_file = "/tmp/projectairsim_voxel_grid.binvox"; + EXPECT_EQ(voxel_request.ncells_x, 20); + EXPECT_EQ(voxel_request.output_file, "/tmp/projectairsim_voxel_grid.binvox"); + + OccupancyGrid::Request occupancy_request; + occupancy_request.position_z = -4.0; + occupancy_request.ncells_x = 20; + occupancy_request.ncells_y = 20; + occupancy_request.ncells_z = 10; + occupancy_request.res = 1.0; + OccupancyGrid::Response occupancy_response; + occupancy_response.success = true; + occupancy_response.map.info.resolution = 1.0F; + occupancy_response.map.info.width = 20; + occupancy_response.map.info.height = 20; + EXPECT_TRUE(occupancy_response.success); + EXPECT_EQ(occupancy_response.map.info.width, 20u); + + GetClock::Response clock_response; + clock_response.nanosec = 123456789; + EXPECT_EQ(clock_response.nanosec, 123456789); + + GetOriginGeoPoint::Response origin_response; + origin_response.latitude = 47.641468; + origin_response.longitude = -122.140165; + origin_response.altitude = 122.0; + EXPECT_NEAR(origin_response.latitude, 47.641468, 1.0e-9); +} + +TEST(Ros2Contracts, GenericClientServiceContracts) { + using projectairsim_ros2_cpp::srv::CancelAllRequests; + using projectairsim_ros2_cpp::srv::GetClientInfo; + using projectairsim_ros2_cpp::srv::GetDrones; + using projectairsim_ros2_cpp::srv::GetTopicInfo; + using projectairsim_ros2_cpp::srv::Publish; + using projectairsim_ros2_cpp::srv::RawRequest; + using projectairsim_ros2_cpp::srv::Unsubscribe; + + RawRequest::Request request; + request.method = "/Sim/GetBuildCommitHash"; + request.json_parameters = "{}"; + RawRequest::Response response; + response.success = true; + response.error_code = 0; + response.result_json = "\"abc123\""; + EXPECT_EQ(request.method, "/Sim/GetBuildCommitHash"); + EXPECT_EQ(response.error_code, 0); + + GetClientInfo::Response client_info; + client_info.client_version = "1.0"; + client_info.nng_version = "1.5"; + client_info.build_commit_hash = "abc123"; + client_info.success = true; + EXPECT_TRUE(client_info.success); + EXPECT_EQ(client_info.build_commit_hash, "abc123"); + + GetTopicInfo::Response topic_info; + topic_info.topics = {"/Sim/topics/example"}; + topic_info.topics_json = "[\"/Sim/topics/example\"]"; + topic_info.success = true; + EXPECT_EQ(topic_info.topics.size(), 1u); + + GetDrones::Response get_drones_response; + get_drones_response.drones = {"Drone1", "Drone2"}; + get_drones_response.success = true; + EXPECT_EQ(get_drones_response.drones[0], "Drone1"); + + Publish::Request publish_request; + publish_request.topic = "/Sim/topics/example"; + publish_request.json_message = "{\"value\":1}"; + Publish::Response publish_response; + publish_response.success = true; + publish_response.status = "OK"; + EXPECT_EQ(publish_request.topic, "/Sim/topics/example"); + EXPECT_TRUE(publish_response.success); + + Unsubscribe::Request unsubscribe_request; + unsubscribe_request.topics = {"/Sim/topics/example"}; + unsubscribe_request.all = false; + Unsubscribe::Response unsubscribe_response; + unsubscribe_response.success = true; + unsubscribe_response.status = "OK"; + EXPECT_FALSE(unsubscribe_request.all); + EXPECT_TRUE(unsubscribe_response.success); + + CancelAllRequests::Response cancel_response; + cancel_response.success = true; + EXPECT_TRUE(cancel_response.success); +} + +TEST(Ros2Contracts, WorldTypedExtensionServiceContracts) { + using projectairsim_ros2_cpp::srv::GetSegmentationIDByName; + using projectairsim_ros2_cpp::srv::GetSegmentationIDMap; + using projectairsim_ros2_cpp::srv::SetObjectMaterial; + using projectairsim_ros2_cpp::srv::SetObjectTexture; + using projectairsim_ros2_cpp::srv::SetSegmentationIDByName; + using projectairsim_ros2_cpp::srv::SwapObjectTexture; + + SetSegmentationIDByName::Request set_seg; + set_seg.mesh_name = "Car.*"; + set_seg.segmentation_id = 42; + set_seg.is_name_regex = true; + set_seg.use_owner_name = false; + EXPECT_TRUE(set_seg.is_name_regex); + + GetSegmentationIDByName::Response get_seg; + get_seg.success = true; + get_seg.segmentation_id = 42; + EXPECT_EQ(get_seg.segmentation_id, 42); + + GetSegmentationIDMap::Response seg_map; + seg_map.success = true; + seg_map.segmentation_map_json = "{}"; + EXPECT_EQ(seg_map.segmentation_map_json, "{}"); + + SetObjectMaterial::Request material; + material.object_name = "Cube"; + material.material_asset_path = "/Game/Materials/M_Test"; + EXPECT_EQ(material.object_name, "Cube"); + + SetObjectTexture::Request texture; + texture.object_name = "Cube"; + texture.source = SetObjectTexture::Request::SOURCE_URL; + texture.texture = "https://example.invalid/texture.png"; + EXPECT_EQ(texture.source, SetObjectTexture::Request::SOURCE_URL); + + SwapObjectTexture::Request swap; + swap.tag = "target"; + swap.tex_id = 2; + EXPECT_EQ(swap.tex_id, 2); +} + +TEST(Ros2Contracts, MoveOnPathActionContract) { + using projectairsim_ros2_cpp::action::MoveOnPath; + + MoveOnPath::Goal goal; + geometry_msgs::msg::PoseStamped waypoint; + waypoint.pose.position.x = 5.0; + waypoint.pose.position.y = 5.0; + waypoint.pose.position.z = -3.0; + waypoint.pose.orientation.w = 1.0; + goal.path.push_back(waypoint); + goal.velocity = 2.0; + goal.timeout_sec = 20.0F; + goal.lookahead = -1.0F; + goal.adaptive_lookahead = 1.0F; + goal.drive_train_type = 0; + goal.yaw_is_rate = true; + goal.yaw = 0.0F; + goal.wait_on_last_task = true; + + MoveOnPath::Feedback feedback; + feedback.current_point_on_path = "submitted"; + + MoveOnPath::Result result; + result.success = true; + + EXPECT_EQ(goal.path.size(), 1u); + EXPECT_EQ(feedback.current_point_on_path, "submitted"); + EXPECT_TRUE(result.success); +} + +TEST(Ros2Contracts, ServiceNameSurfaceMatchesBridgeDefaults) { + const std::vector services = { + "/projectairsim/request", + "/projectairsim/get_client_info", + "/projectairsim/get_topic_info", + "/projectairsim/get_drones", + "/projectairsim/publish", + "/projectairsim/unsubscribe", + "/projectairsim/cancel_all_requests", + "/projectairsim/Drone1/takeoff", + "/projectairsim/Drone1/land", + "/projectairsim/Drone1/arm", + "/projectairsim/Drone1/disarm", + "/projectairsim/Drone1/move_to_position", + "/projectairsim/Drone1/move_on_path", + "/projectairsim/takeoff_group", + "/projectairsim/land_group", + "/projectairsim/arm_group", + "/projectairsim/disarm_group", + "/projectairsim/reset", + "/projectairsim/load_scene", + "/projectairsim/create_voxel_grid", + "/projectairsim/occupancy_grid", + "/projectairsim/get_clock", + "/projectairsim/get_origin_geo_point", + "/projectairsim/set_segmentation_id_by_name", + "/projectairsim/get_segmentation_id_by_name", + "/projectairsim/get_segmentation_id_map", + "/projectairsim/set_object_material", + "/projectairsim/set_object_texture", + "/projectairsim/swap_object_texture", + }; + + EXPECT_EQ(services.size(), 29u); + EXPECT_EQ(services.front(), "/projectairsim/request"); + EXPECT_EQ(services.back(), "/projectairsim/swap_object_texture"); +} + +} // namespace diff --git a/ros/projectairsim_ros2_cpp/test/ros2_unreal_integration_test.cpp b/ros/projectairsim_ros2_cpp/test/ros2_unreal_integration_test.cpp new file mode 100644 index 00000000..925e23fa --- /dev/null +++ b/ros/projectairsim_ros2_cpp/test/ros2_unreal_integration_test.cpp @@ -0,0 +1,360 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "projectairsim_ros2_cpp/srv/get_clock.hpp" +#include "projectairsim_ros2_cpp/srv/get_drones.hpp" +#include "projectairsim_ros2_cpp/srv/land_group.hpp" +#include "projectairsim_ros2_cpp/srv/load_scene.hpp" +#include "projectairsim_ros2_cpp/srv/move_on_path.hpp" +#include "projectairsim_ros2_cpp/srv/takeoff_group.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rosgraph_msgs/msg/clock.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" + +using namespace std::chrono_literals; + +namespace { + +constexpr const char* kServiceRoot = "/projectairsim"; +constexpr const char* kRosTopicRoot = "/ProjectAirsim"; + +class RclcppEnvironment : public ::testing::Environment { + public: + void SetUp() override { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + void TearDown() override { + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + } +}; + +template +typename ServiceT::Response::SharedPtr CallService( + const rclcpp::Node::SharedPtr& node, const std::string& service_name, + const typename ServiceT::Request& request, std::chrono::seconds timeout = 30s) { + auto client = node->create_client(service_name); + if (!client->wait_for_service(timeout)) { + ADD_FAILURE() << "Service not available: " << service_name + << ". Start Unreal and projectairsim_ros2_cpp_node first."; + return nullptr; + } + + auto future = + client->async_send_request(std::make_shared(request)); + const auto result = + rclcpp::spin_until_future_complete(node, future, timeout); + if (result != rclcpp::FutureReturnCode::SUCCESS) { + ADD_FAILURE() << "Timed out calling service: " << service_name; + return nullptr; + } + return future.get(); +} + +bool LoadScene(const rclcpp::Node::SharedPtr& node, const std::string& scene_file) { + projectairsim_ros2_cpp::srv::LoadScene::Request request; + request.scene_file = scene_file; + request.is_primary_client = true; + + for (int attempt = 0; attempt < 5; ++attempt) { + const auto response = CallService( + node, std::string(kServiceRoot) + "/load_scene", request, 60s); + if (response && response->success) return true; + std::this_thread::sleep_for(1s); + } + return false; +} + +std::map> Topics( + const rclcpp::Node::SharedPtr& node) { + return node->get_topic_names_and_types(); +} + +bool HasTopicType(const rclcpp::Node::SharedPtr& node, const std::string& topic, + const std::string& type) { + for (const auto& [topic_name, topic_types] : Topics(node)) { + if (topic_name != topic) continue; + return std::find(topic_types.begin(), topic_types.end(), type) != + topic_types.end(); + } + return false; +} + +bool HasTopicEndingWithType(const rclcpp::Node::SharedPtr& node, + const std::string& suffix, const std::string& type, + std::string* matched_topic = nullptr) { + for (const auto& [topic_name, topic_types] : Topics(node)) { + if (topic_name.size() < suffix.size() || + topic_name.compare(topic_name.size() - suffix.size(), suffix.size(), + suffix) != 0) { + continue; + } + if (std::find(topic_types.begin(), topic_types.end(), type) == + topic_types.end()) { + continue; + } + if (matched_topic) *matched_topic = topic_name; + return true; + } + return false; +} + +bool WaitForTopicEndingWithType(const rclcpp::Node::SharedPtr& node, + const std::string& suffix, + const std::string& type, + std::string* matched_topic = nullptr, + std::chrono::seconds timeout = 30s) { + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (HasTopicEndingWithType(node, suffix, type, matched_topic)) return true; + rclcpp::spin_some(node); + std::this_thread::sleep_for(250ms); + } + return false; +} + +template +std::vector CaptureMessages( + const rclcpp::Node::SharedPtr& node, const std::string& topic, + size_t min_count, std::chrono::seconds timeout = 30s) { + std::vector messages; + auto subscription = node->create_subscription( + topic, rclcpp::SensorDataQoS(), + [&messages](typename MsgT::SharedPtr msg) { messages.push_back(msg); }); + + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (messages.size() < min_count && std::chrono::steady_clock::now() < deadline) { + rclcpp::spin_some(node); + std::this_thread::sleep_for(100ms); + } + return messages; +} + +geometry_msgs::msg::PoseStamped::SharedPtr CapturePose( + const rclcpp::Node::SharedPtr& node, const std::string& pose_topic) { + const auto poses = + CaptureMessages(node, pose_topic, 1, 20s); + if (poses.empty()) return nullptr; + return poses.front(); +} + +std::vector TopicsContaining(const rclcpp::Node::SharedPtr& node, + const std::vector& needles, + const std::string& type) { + std::vector matches; + for (const auto& [topic_name, topic_types] : Topics(node)) { + const auto has_type = + std::find(topic_types.begin(), topic_types.end(), type) != topic_types.end(); + if (!has_type) continue; + + for (const auto& needle : needles) { + if (topic_name.find(needle) != std::string::npos) { + matches.push_back(topic_name); + break; + } + } + } + std::sort(matches.begin(), matches.end()); + matches.erase(std::unique(matches.begin(), matches.end()), matches.end()); + return matches; +} + +std::vector MessageTimes(const std::vector& messages) { + std::vector times; + times.reserve(messages.size()); + for (const auto& msg : messages) { + times.push_back(static_cast(msg->header.stamp.sec) * 1000000000LL + + static_cast(msg->header.stamp.nanosec)); + } + return times; +} + +geometry_msgs::msg::PoseStamped Waypoint(double x, double y, double z) { + geometry_msgs::msg::PoseStamped waypoint; + waypoint.header.frame_id = "world"; + waypoint.pose.position.x = x; + waypoint.pose.position.y = y; + waypoint.pose.position.z = z; + waypoint.pose.orientation.w = 1.0; + return waypoint; +} + +class UnrealIntegrationTest : public ::testing::Test { + protected: + void SetUp() override { + node_ = rclcpp::Node::make_shared("projectairsim_ros2_cpp_unreal_test"); + } + + void TearDown() override { node_.reset(); } + + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(UnrealIntegrationTest, BasicScenePublishesStandardTopicsAndClock) { + ASSERT_TRUE(LoadScene(node_, "scene_basic_drone.jsonc")); + + std::string actual_pose_topic; + ASSERT_TRUE(WaitForTopicEndingWithType( + node_, "/robots/Drone1/actual_pose", "geometry_msgs/msg/PoseStamped", + &actual_pose_topic, 45s)); + + EXPECT_TRUE(HasTopicType(node_, "/clock", "rosgraph_msgs/msg/Clock")); + EXPECT_TRUE(HasTopicEndingWithType(node_, "/gps", "sensor_msgs/msg/NavSatFix")); + EXPECT_TRUE(HasTopicEndingWithType(node_, "/imu", "sensor_msgs/msg/Imu")); + + const auto clock_messages = + CaptureMessages(node_, "/clock", 2, 20s); + ASSERT_GE(clock_messages.size(), 2u); + const auto first_time = static_cast(clock_messages.front()->clock.sec) * + 1000000000LL + + clock_messages.front()->clock.nanosec; + const auto last_time = static_cast(clock_messages.back()->clock.sec) * + 1000000000LL + + clock_messages.back()->clock.nanosec; + EXPECT_GT(last_time, first_time); + + const auto pose = CapturePose(node_, actual_pose_topic); + ASSERT_NE(pose, nullptr); + EXPECT_TRUE(std::isfinite(pose->pose.position.x)); + EXPECT_TRUE(std::isfinite(pose->pose.position.y)); + EXPECT_TRUE(std::isfinite(pose->pose.position.z)); +} + +TEST_F(UnrealIntegrationTest, DroneSensorScenePublishesImageAndCameraInfoTopics) { + ASSERT_TRUE(LoadScene(node_, "scene_drone_sensors.jsonc")); + + const std::vector image_needles = { + "DownCamera/depth_camera", + "DownCamera/scene_camera", + "DownCamera/segmentation_camera", + }; + + std::vector image_topics; + const auto deadline = std::chrono::steady_clock::now() + 45s; + while (std::chrono::steady_clock::now() < deadline) { + image_topics = + TopicsContaining(node_, image_needles, "sensor_msgs/msg/Image"); + if (image_topics.size() >= 3) break; + rclcpp::spin_some(node_); + std::this_thread::sleep_for(500ms); + } + + ASSERT_GE(image_topics.size(), 3u); + for (const auto& image_topic : image_topics) { + EXPECT_TRUE(HasTopicType(node_, image_topic + "/camera_info", + "sensor_msgs/msg/CameraInfo")) + << "Missing camera_info for " << image_topic; + } + + for (const auto& image_topic : image_topics) { + const auto messages = + CaptureMessages(node_, image_topic, 3, 45s); + ASSERT_GE(messages.size(), 1u) << "No image messages on " << image_topic; + EXPECT_GT(messages.front()->width, 0u); + EXPECT_GT(messages.front()->height, 0u); + } +} + +TEST_F(UnrealIntegrationTest, TakeoffChangesActualPose) { + ASSERT_TRUE(LoadScene(node_, "scene_basic_drone.jsonc")); + + std::string actual_pose_topic; + ASSERT_TRUE(WaitForTopicEndingWithType( + node_, "/robots/Drone1/actual_pose", "geometry_msgs/msg/PoseStamped", + &actual_pose_topic, 45s)); + + const auto before = CapturePose(node_, actual_pose_topic); + ASSERT_NE(before, nullptr); + const auto z_before = before->pose.position.z; + + projectairsim_ros2_cpp::srv::TakeoffGroup::Request request; + request.vehicle_names = {"Drone1"}; + request.wait_on_last_task = false; + const auto response = CallService( + node_, std::string(kServiceRoot) + "/takeoff_group", request, 60s); + ASSERT_NE(response, nullptr); + ASSERT_TRUE(response->success); + + std::this_thread::sleep_for(5s); + const auto after = CapturePose(node_, actual_pose_topic); + ASSERT_NE(after, nullptr); + EXPECT_NE(after->pose.position.z, z_before); + EXPECT_GT(after->pose.position.z, z_before); + + projectairsim_ros2_cpp::srv::LandGroup::Request land_request; + land_request.vehicle_names = {"Drone1"}; + land_request.wait_on_last_task = false; + (void)CallService( + node_, std::string(kServiceRoot) + "/land_group", land_request, 60s); +} + +TEST_F(UnrealIntegrationTest, TwoDroneSceneSupportsGroupTakeoffMoveAndLand) { + ASSERT_TRUE(LoadScene(node_, "scene_two_drones.jsonc")); + + projectairsim_ros2_cpp::srv::GetDrones::Request drones_request; + const auto drones_response = CallService( + node_, std::string(kServiceRoot) + "/get_drones", drones_request, 30s); + ASSERT_NE(drones_response, nullptr); + ASSERT_TRUE(drones_response->success); + const std::set drones(drones_response->drones.begin(), + drones_response->drones.end()); + EXPECT_TRUE(drones.count("Drone1")); + EXPECT_TRUE(drones.count("Drone2")); + + projectairsim_ros2_cpp::srv::TakeoffGroup::Request takeoff_request; + takeoff_request.vehicle_names = {"Drone1", "Drone2"}; + takeoff_request.wait_on_last_task = true; + const auto takeoff_response = + CallService( + node_, std::string(kServiceRoot) + "/takeoff_group", takeoff_request, 90s); + ASSERT_NE(takeoff_response, nullptr); + ASSERT_TRUE(takeoff_response->success); + + projectairsim_ros2_cpp::srv::MoveOnPath::Request move_request; + move_request.path = {Waypoint(30.0, 0.0, -30.0), Waypoint(0.0, 0.0, -8.0)}; + move_request.velocity = 10.0; + move_request.timeout_sec = 45.0F; + move_request.drive_train_type = 0; + move_request.yaw_is_rate = true; + move_request.yaw = 0.0F; + move_request.lookahead = -1.0F; + move_request.adaptive_lookahead = 1.0F; + move_request.wait_on_last_task = false; + const auto move_response = CallService( + node_, std::string(kServiceRoot) + "/Drone1/move_on_path", move_request, 60s); + ASSERT_NE(move_response, nullptr); + EXPECT_TRUE(move_response->success); + + projectairsim_ros2_cpp::srv::LandGroup::Request land_request; + land_request.vehicle_names = {"Drone1", "Drone2"}; + land_request.wait_on_last_task = false; + const auto land_response = CallService( + node_, std::string(kServiceRoot) + "/land_group", land_request, 30s); + ASSERT_NE(land_response, nullptr); + EXPECT_TRUE(land_response->success); +} + +} // namespace + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + ::testing::AddGlobalTestEnvironment(new RclcppEnvironment()); + return RUN_ALL_TESTS(); +}