From f093a33af81eed033e733a7e1f5f62336dfe72b7 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 19 Apr 2024 20:55:31 -0600 Subject: [PATCH 1/2] Add SITL viewer model Signed-off-by: Ryan Friedman --- .../models/iris_sitl_viewer/model.config | 24 ++++ .../models/iris_sitl_viewer/model.sdf | 83 +++++++++++++ ardupilot_gz_gazebo/package.xml | 1 + ardupilot_gz_gazebo/worlds/iris_viewer.sdf | 117 ++++++++++++++++++ 4 files changed, 225 insertions(+) create mode 100644 ardupilot_gz_description/models/iris_sitl_viewer/model.config create mode 100644 ardupilot_gz_description/models/iris_sitl_viewer/model.sdf create mode 100644 ardupilot_gz_gazebo/worlds/iris_viewer.sdf 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 + + + + + + From 86eff9fc86fb559e36e9b41da96100c99d89fcbc Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 20 Apr 2024 08:19:35 -0600 Subject: [PATCH 2/2] Move SITL viewer to ardupilot_gz Signed-off-by: Ryan Friedman --- ardupilot_gz_sitl_viewer/CMakeLists.txt | 135 ++++++++++++++++++ ardupilot_gz_sitl_viewer/README.md | 24 ++++ .../hooks/ardupilot_gz_sitl_viewer.dsv.in | 4 + .../hooks/ardupilot_gz_sitl_viewer.sh.in | 3 + .../ardupilot_gz_sitl_viewer/SitlViewer.hpp | 39 +++++ ardupilot_gz_sitl_viewer/package.xml | 32 +++++ ardupilot_gz_sitl_viewer/src/SitlViewer.cpp | 49 +++++++ .../src/SitlViewerNode.cpp | 11 ++ 8 files changed, 297 insertions(+) create mode 100644 ardupilot_gz_sitl_viewer/CMakeLists.txt create mode 100644 ardupilot_gz_sitl_viewer/README.md create mode 100644 ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.dsv.in create mode 100644 ardupilot_gz_sitl_viewer/hooks/ardupilot_gz_sitl_viewer.sh.in create mode 100644 ardupilot_gz_sitl_viewer/include/ardupilot_gz_sitl_viewer/SitlViewer.hpp create mode 100644 ardupilot_gz_sitl_viewer/package.xml create mode 100644 ardupilot_gz_sitl_viewer/src/SitlViewer.cpp create mode 100644 ardupilot_gz_sitl_viewer/src/SitlViewerNode.cpp 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; +} +