Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add SITL viewer model #51

Draft
wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Add SITL viewer model #51

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f093a33
Add SITL viewer model
Ryanf55 Apr 20, 2024
86eff9f
Move SITL viewer to ardupilot_gz
Ryanf55 Apr 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 24 additions & 0 deletions ardupilot_gz_description/models/iris_sitl_viewer/model.config
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
<?xml version="1.0"?>

<model>
<name>Iris SITL Viewer</name>
<version>1.0</version>
<sdf version="1.9">model.sdf</sdf>

<author>
<name>Ryan Friedman</name>
<email>[email protected]</email>
</author>

<maintainer email="[email protected]">Ryan Friedman</maintainer>

<description>
View-only Iris copter for showing ArduPilot SITL FDM state in Gazebo.
</description>
<depend>
<model>
<uri>model://iris_sitl_viewer</uri>
<version>1.0</version>
</model>
</depend>
</model>
83 changes: 83 additions & 0 deletions ardupilot_gz_description/models/iris_sitl_viewer/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
<?xml version='1.0'?>
<sdf version="1.9">

<model name="iris_sitl_viewer">
<include merge="true">
<uri>model://iris_with_standoffs</uri>
</include>

<link name="base_scan">
<pose>0 0 0.075077 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>

<collision name="collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>

<visual name="visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</visual>

<sensor name='gpu_lidar' type='gpu_lidar'>
<gz_frame_id>base_scan</gz_frame_id>
<pose>0 0 0 0 0 0</pose>
<topic>lidar</topic>
<update_rate>10</update_rate>
<lidar>
<scan>
<horizontal>
<samples>640</samples>
<resolution>1</resolution>
<min_angle>-3.14159265</min_angle>
<max_angle>3.14159265</max_angle>
</horizontal>
<vertical>
<samples>1</samples>
<resolution>1</resolution>
<min_angle>0.0</min_angle>
<max_angle>0.0</max_angle>
</vertical>
</scan>
<range>
<min>0.08</min>
<max>10.0</max>
<resolution>0.01</resolution>
</range>
</lidar>
<visualize>true</visualize>
</sensor>
</link>

<joint name="lidar_joint" type="revolute">
<parent>base_link</parent>
<child>base_scan</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<dynamics>
<damping>1</damping>
</dynamics>
</axis>
</joint>

</model>

</sdf>
1 change: 1 addition & 0 deletions ardupilot_gz_gazebo/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
<depend>gz-common5</depend>
<depend>gz-cmake3</depend>
<depend>gz-plugin2</depend>
<depend>ardupilot_gz_description</depend>

<!-- Garden (default) -->
<depend condition="$GZ_VERSION == garden">gz-sim7</depend>
Expand Down
117 changes: 117 additions & 0 deletions ardupilot_gz_gazebo/worlds/iris_viewer.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,117 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<world name="map">
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>

<plugin filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin filename="gz-sim-air-pressure-system"
name="gz::sim::systems::AirPressure">
</plugin>
<plugin filename="gz-sim-air-speed-system"
name="gz::sim::systems::AirSpeed">
</plugin>
<plugin filename="gz-sim-altimeter-system"
name="gz::sim::systems::Altimeter">
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-magnetometer-system"
name="gz::sim::systems::Magnetometer">
</plugin>
<plugin filename="gz-sim-navsat-system"
name="gz::sim::systems::NavSat">
</plugin>

<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
<sky></sky>
</scene>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<spherical_coordinates>
<latitude_deg>-35.3632621</latitude_deg>
<longitude_deg>149.1652374</longitude_deg>
<elevation>584</elevation>
<heading_deg>0</heading_deg>
<surface_model>EARTH_WGS84</surface_model>
</spherical_coordinates>

<!-- <include>
<uri>model://runway</uri>
<pose degrees="true">-29 545 0 0 0 363</pose>
</include> -->

<!-- <include>
<uri>model://iris_sitl_viewer</uri>
<name>iris</name>
<pose degrees="true">0 0 0.195 0 0 90</pose>
</include> -->

<model name="NavSat">
<pose>0 0 0.05 0 0.0 0</pose>
<!-- <static>true</static> -->
<link name="link">
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.5 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.2</size>
</box>
</geometry>
</visual>

<sensor name="navsat" type="navsat">
<always_on>1</always_on>
<update_rate>1</update_rate>
<topic>navsat</topic>
</sensor>
</link>
</model>

</world>
</sdf>
135 changes: 135 additions & 0 deletions ardupilot_gz_sitl_viewer/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,135 @@
cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The intention is for system plugins such as the SitlViewer to go under ardupilot_gz_gazebo/src in their own sub-folder. This is how gz-sim is organised, where there is a gz-sim/src/systems directory. ardupilot_gz_gazebo already has hooks, so should install any system plugins to the appropriate {workpace}/install location.

The supplied examples (from the template) should really be under src/systems/basic_system and src/systems/full_system. They can be relocated as a separate PR to make the intent clearer.

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()
24 changes: 24 additions & 0 deletions ardupilot_gz_sitl_viewer/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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?

Loading