diff --git a/ardupilot_gz_description/models/iris_sitl_viewer/model.config b/ardupilot_gz_description/models/iris_sitl_viewer/model.config
new file mode 100644
index 0000000..80ace61
--- /dev/null
+++ b/ardupilot_gz_description/models/iris_sitl_viewer/model.config
@@ -0,0 +1,24 @@
+
+
+
+ Iris SITL Viewer
+ 1.0
+ model.sdf
+
+
+ Ryan Friedman
+ ryanfriedman5410+github@gmail.com
+
+
+ Ryan Friedman
+
+
+ View-only Iris copter for showing ArduPilot SITL FDM state in Gazebo.
+
+
+
+ model://iris_sitl_viewer
+ 1.0
+
+
+
\ No newline at end of file
diff --git a/ardupilot_gz_description/models/iris_sitl_viewer/model.sdf b/ardupilot_gz_description/models/iris_sitl_viewer/model.sdf
new file mode 100644
index 0000000..5f24a87
--- /dev/null
+++ b/ardupilot_gz_description/models/iris_sitl_viewer/model.sdf
@@ -0,0 +1,83 @@
+
+
+
+
+
+ model://iris_with_standoffs
+
+
+
+ 0 0 0.075077 0 0 0
+
+ 0.1
+
+ 0.000166667
+ 0.000166667
+ 0.000166667
+
+
+
+
+
+
+ 0.05 0.05 0.05
+
+
+
+
+
+
+
+ 0.05 0.05 0.05
+
+
+
+
+
+ base_scan
+ 0 0 0 0 0 0
+ lidar
+ 10
+
+
+
+ 640
+ 1
+ -3.14159265
+ 3.14159265
+
+
+ 1
+ 1
+ 0.0
+ 0.0
+
+
+
+ 0.08
+ 10.0
+ 0.01
+
+
+ true
+
+
+
+
+ base_link
+ base_scan
+
+ 0 0 1
+
+ 0
+ 0
+
+
+ 1
+
+
+
+
+
+
+
diff --git a/ardupilot_gz_gazebo/package.xml b/ardupilot_gz_gazebo/package.xml
index d2eacb3..a85cdf9 100644
--- a/ardupilot_gz_gazebo/package.xml
+++ b/ardupilot_gz_gazebo/package.xml
@@ -11,6 +11,7 @@
gz-common5
gz-cmake3
gz-plugin2
+ ardupilot_gz_description
gz-sim7
diff --git a/ardupilot_gz_gazebo/worlds/iris_viewer.sdf b/ardupilot_gz_gazebo/worlds/iris_viewer.sdf
new file mode 100644
index 0000000..a868bfa
--- /dev/null
+++ b/ardupilot_gz_gazebo/worlds/iris_viewer.sdf
@@ -0,0 +1,117 @@
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+ ogre2
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1.0 1.0 1.0
+ 0.8 0.8 0.8
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ -35.3632621
+ 149.1652374
+ 584
+ 0
+ EARTH_WGS84
+
+
+
+
+
+
+
+ 0 0 0.05 0 0.0 0
+
+
+
+ 0.1
+
+ 0.000166667
+ 0.000166667
+ 0.000166667
+
+
+
+
+
+ 0.1 0.5 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.2
+
+
+
+
+
+ 1
+ 1
+ navsat
+
+
+
+
+
+
diff --git a/ardupilot_gz_sitl_viewer/CMakeLists.txt b/ardupilot_gz_sitl_viewer/CMakeLists.txt
new file mode 100644
index 0000000..ccca02f
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/CMakeLists.txt
@@ -0,0 +1,135 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+project(ardupilot_gz_sitl_viewer)
+
+# --------------------------------------------------------------------------- #
+# If ament_cmake is found build as an ament package, otherwise ignore.
+# This is so the system may be built for Gazebo only, if ROS is not available.
+find_package(ament_cmake QUIET)
+if(${ament_cmake_FOUND})
+ message("Building ${PROJECT_NAME} as an `ament_cmake` project.")
+endif()
+
+# --------------------------------------------------------------------------- #
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# --------------------------------------------------------------------------- #
+# Find gz-sim and dependencies.
+
+# Harmonic
+if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
+ find_package(gz-cmake3 REQUIRED)
+ set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR})
+
+ gz_find_package(gz-common5 REQUIRED)
+ set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
+
+ gz_find_package(gz-rendering8 REQUIRED)
+ set(GZ_RENDERING_VER ${gz-rendering8_VERSION_MAJOR})
+
+ gz_find_package(gz-sim8 REQUIRED)
+ set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
+
+ gz_find_package(gz-transport13 REQUIRED)
+ set(GZ_TRANSPORT_VER ${gz-transport13_VERSION_MAJOR})
+
+ message(STATUS "Compiling against Gazebo Harmonic")
+# Garden (default)
+elseif("$ENV{GZ_VERSION}" STREQUAL "garden" OR NOT DEFINED "ENV{GZ_VERSION}")
+ find_package(gz-cmake3 REQUIRED)
+ set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR})
+
+ gz_find_package(gz-common5 REQUIRED)
+ set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
+
+ gz_find_package(gz-rendering7 REQUIRED)
+ set(GZ_RENDERING_VER ${gz-rendering7_VERSION_MAJOR})
+
+ gz_find_package(gz-sim7 REQUIRED)
+ set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
+
+ # TODO add gz-transport
+
+ message(STATUS "Compiling against Gazebo Garden")
+else()
+ message(FATAL_ERROR "Unsupported GZ_VERSION: $ENV{GZ_VERSION}")
+endif()
+
+# --------------------------------------------------------------------------- #
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# --------------------------------------------------------------------------- #
+# Build plugin.
+
+add_library(SitlViewer
+ src/SitlViewer.cpp
+)
+add_library(SitlViewer::SitlViewer ALIAS SitlViewer)
+
+add_executable(SitlViewerNode src/SitlViewerNode.cpp)
+
+# TODO support proper ament install
+target_include_directories(SitlViewer PUBLIC
+ include/${PROJEC}
+)
+
+target_link_libraries(SitlViewer PUBLIC
+ gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER}
+ rclcpp::rclcpp
+ ${sensor_msgs_TARGETS}
+)
+
+target_link_libraries(SitlViewerNode PUBLIC
+ SitlViewer::SitlViewer
+ rclcpp::rclcpp
+)
+
+# --------------------------------------------------------------------------- #
+# Install.
+
+install(
+ TARGETS
+ SitlViewer
+ EXPORT export_${PROJECT_NAME}
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib
+ RUNTIME DESTINATION bin
+)
+
+install(TARGETS SitlViewerNode
+ DESTINATION lib/${PROJECT_NAME})
+
+install(
+ DIRECTORY include/
+ DESTINATION include/${PROJECT_NAME}
+)
+
+#install(
+# DIRECTORY
+# config/
+# DESTINATION share/${PROJECT_NAME}/config
+#)
+
+#install(
+# DIRECTORY
+# models/
+# DESTINATION share/${PROJECT_NAME}/models
+#)
+
+#install(
+# DIRECTORY
+# worlds/
+# DESTINATION share/${PROJECT_NAME}/worlds
+#)
+
+# --------------------------------------------------------------------------- #
+# Register as an ament package if ament_cmake is available.
+if(${ament_cmake_FOUND})
+ ament_environment_hooks(
+ "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
+ ament_environment_hooks(
+ "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
+
+ ament_package()
+endif()
\ No newline at end of file
diff --git a/ardupilot_gz_sitl_viewer/README.md b/ardupilot_gz_sitl_viewer/README.md
new file mode 100644
index 0000000..e45a35c
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/README.md
@@ -0,0 +1,24 @@
+# ArduPilot Gazebo SITL Viewer
+
+This is a simpler approach to Gazebo where ArduPilot SITL runs the FDM.
+Gazebo is a 3D viewer for the vehicle compared to MAVProxy or a GCS.
+This approach allows for easy extension of the Gazebo environment to support other
+aspects of the environment and is useful if you don't want to simulate the vehicle physics
+or collisions with terrain.
+
+ArduPilot SITL outputs state data via DDS to the SITL Viewer.
+SITL viewer converts the data to Gazebo's protobuf transport.
+Gazebo currently does not send any data back to ArduPilot.
+
+## Message Architecture
+
+* NavSatFix
+ * Source: Ardupilot ROS sensor_msgs::msg::NavSatFix
+ * Destination: Call the Gazebo TODO plugin `set_spherical_coordinates` service
+ * This updates the position of the "vehicle"
+* tf_static
+ * Source: ArduPilot ROS static transforms tf2_msgs::msg::TFMessage
+ * Destination: TODO
+ * TODO this should update the home position of the world
+ * TODO can we dynamically load terrain too from the AP terrain server?
+
diff --git a/ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.dsv.in b/ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.dsv.in
new file mode 100644
index 0000000..f5341ea
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.dsv.in
@@ -0,0 +1,4 @@
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share;@CMAKE_INSTALL_PREFIX@/share
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/models
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/worlds
+prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/
\ No newline at end of file
diff --git a/ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.sh.in b/ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.sh.in
new file mode 100644
index 0000000..6156ee3
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.sh.in
@@ -0,0 +1,3 @@
+ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/models"
+ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/worlds"
+ament_prepend_unique_value GZ_SIM_PLUGIN_PATH "$AMENT_CURRENT_PREFIX/lib/@PROJECT_NAME@"
\ No newline at end of file
diff --git a/ardupilot_gz_sitl_viewer/include/ardupilot_gz_sitl_viewer/SitlViewer.hpp b/ardupilot_gz_sitl_viewer/include/ardupilot_gz_sitl_viewer/SitlViewer.hpp
new file mode 100644
index 0000000..1b03f56
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/include/ardupilot_gz_sitl_viewer/SitlViewer.hpp
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2016 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include
+#include
+#include
+#include
+#include
+
+//! @class This class creates a gazebo world on the fly to view SITL.
+//! It interfaces with the DDS interface of Ardupilot.
+//! It relies on the ArduPilot SITL FDM for the aircraft dynamics.
+class SitlViewer: public rclcpp::Node
+ {
+ public:
+ SitlViewer();
+ private:
+
+ void OnNavSatFix(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
+ rclcpp::Subscription::SharedPtr navsat_sub_;
+ gz::transport::Node gz_node_;
+ const unsigned int gz_service_timeout_ms_ {5000};
+ std::string set_spherical_coords_topic {"/world/map/set_spherical_coordinates"};
+
+};
\ No newline at end of file
diff --git a/ardupilot_gz_sitl_viewer/package.xml b/ardupilot_gz_sitl_viewer/package.xml
new file mode 100644
index 0000000..bf0fcec
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/package.xml
@@ -0,0 +1,32 @@
+
+
+
+ ardupilot_gz_sitl_viewer
+ 0.0.0
+ Simulation Adapter for ArduPilot SITL FDM to ve vizualized in Gazebo using ROS
+ Ryan Friedman
+ GPL-3.0
+ Ryan Friedman
+
+ ament_cmake
+
+ rclcpp
+ sensor_msgs
+
+
+
+ gz-cmake3
+ gz-sim8
+ gz-transport13
+
+ gz-cmake3
+ gz-sim7
+ gz-cmake3
+ gz-sim7
+
+ ament_lint_auto
+
+
+ ament_cmake
+
+
diff --git a/ardupilot_gz_sitl_viewer/src/SitlViewer.cpp b/ardupilot_gz_sitl_viewer/src/SitlViewer.cpp
new file mode 100644
index 0000000..b25f99f
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/src/SitlViewer.cpp
@@ -0,0 +1,49 @@
+#include "ardupilot_gz_sitl_viewer/SitlViewer.hpp"
+#include
+
+SitlViewer::SitlViewer() : Node("SitlViewer") {
+
+ rmw_qos_profile_t qos_sensor;
+ qos_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+ navsat_sub_ = create_subscription(
+ "/ap/navsat/navsat0",
+ rclcpp::SensorDataQoS(),
+ [this](std::shared_ptr msg) {
+ OnNavSatFix(msg);
+ }
+ );
+}
+
+void SitlViewer::OnNavSatFix(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
+{
+ RCLCPP_INFO(this->get_logger(), "Got NavSatFix from AP");
+
+ gz::msgs::SphericalCoordinates req;
+ req.set_elevation(msg->altitude);
+ req.set_longitude_deg(msg->longitude);
+ req.set_latitude_deg(msg->latitude);
+
+
+ //! @todo Dynamically configure entity name based on frame_id from AP.
+ //! @todo Spawn the entity if it doesn't exist.
+ auto entity = req.mutable_entity();
+ assert(entity != nullptr);
+ // gz::msgs::Entity entity;
+ entity->set_name("NavSat");
+ entity->set_type(gz::msgs::Entity_Type_MODEL);
+ // req.entity(entity);
+
+ gz::msgs::Boolean rep;
+ bool result;
+ bool executed = gz_node_.Request("/world/map/set_spherical_coordinates", req, gz_service_timeout_ms_, rep, result);
+ if (executed)
+ {
+ if (result)
+ RCLCPP_INFO_STREAM(this->get_logger(), "Response: [" << rep.data() << "]");
+ else
+ RCLCPP_ERROR(this->get_logger(), "Service call failed");
+ }
+ else {
+ RCLCPP_ERROR(this->get_logger(), "Service call timed out");
+ }
+}
diff --git a/ardupilot_gz_sitl_viewer/src/SitlViewerNode.cpp b/ardupilot_gz_sitl_viewer/src/SitlViewerNode.cpp
new file mode 100644
index 0000000..1ee34dd
--- /dev/null
+++ b/ardupilot_gz_sitl_viewer/src/SitlViewerNode.cpp
@@ -0,0 +1,11 @@
+#include "rclcpp/rclcpp.hpp"
+#include "ardupilot_gz_sitl_viewer/SitlViewer.hpp"
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
+