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(); +}