Move SITL viewer to ardupilot_gz

Signed-off-by: Ryan Friedman <[email protected]>

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()

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?
+

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@/

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@"

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 <string>
+#include <memory>
+#include <gz/transport.hh>
+#include <rclcpp/node.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+
+//! @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<sensor_msgs::msg::NavSatFix>::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"};
+
+};

ardupilot_gz_sitl_viewer/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ardupilot_gz_sitl_viewer</name>
+  <version>0.0.0</version>
+  <description>Simulation Adapter for ArduPilot SITL FDM to ve vizualized in Gazebo using ROS</description>
+  <maintainer email="[email protected]">Ryan Friedman</maintainer>
+  <license>GPL-3.0</license>
+  <author>Ryan Friedman</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+
+
+  <!-- Harmonic -->
+  <depend condition="$GZ_VERSION == harmonic">gz-cmake3</depend>
+  <depend condition="$GZ_VERSION == harmonic">gz-sim8</depend>
+  <depend condition="$GZ_VERSION == harmonic">gz-transport13</depend>
+  <!-- Garden (default) -->
+  <depend condition="$GZ_VERSION == garden">gz-cmake3</depend>
+  <depend condition="$GZ_VERSION == garden">gz-sim7</depend>
+  <depend condition="$GZ_VERSION == ''">gz-cmake3</depend>
+  <depend condition="$GZ_VERSION == ''">gz-sim7</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ardupilot_gz_sitl_viewer/src/SitlViewer.cpp

@@ -0,0 +1,49 @@
+#include "ardupilot_gz_sitl_viewer/SitlViewer.hpp"
+#include <gz/msgs.hh>
+
+SitlViewer::SitlViewer() : Node("SitlViewer") {
+
+    rmw_qos_profile_t qos_sensor;
+    qos_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+    navsat_sub_ = create_subscription<sensor_msgs::msg::NavSatFix>(
+        "/ap/navsat/navsat0",
+        rclcpp::SensorDataQoS(),
+        [this](std::shared_ptr<sensor_msgs::msg::NavSatFix> 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");
+    }
+}

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<SitlViewer>());
+  rclcpp::shutdown();
+  return 0;
+}
+