Skip to content

Commit

Permalink
adapt to brand new plugin infrastructure
Browse files Browse the repository at this point in the history
  • Loading branch information
mbjd committed Mar 6, 2025
1 parent b344f55 commit a9b6ffb
Show file tree
Hide file tree
Showing 5 changed files with 125 additions and 16 deletions.
3 changes: 2 additions & 1 deletion src/modules/simulation/gz_plugins/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,8 @@ if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND)
# Add our plugins as subdirectories
add_subdirectory(optical_flow)
add_subdirectory(template_plugin)
add_subdirectory(moving_platform_controller)

# Add an alias target for each plugin
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin)
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin)
endif()
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
############################################################################
#
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################

# Template for a new plugin project
# Replace TemplatePlugin with your plugin name
project(MovingPlatformController)

# Add external dependencies if needed
# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)

# Find required packages
# find_package(PackageName REQUIRED)

add_library(${PROJECT_NAME} SHARED
# Add your source files here
MovingPlatformController.cpp
)

target_link_libraries(${PROJECT_NAME}
PUBLIC px4_gz_msgs
PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION}
PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION}
PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION}
PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION}
# Add other dependencies as needed
# PUBLIC ${OtherLib_LIBS}
)

target_include_directories(${PROJECT_NAME}
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
PUBLIC px4_gz_msgs
# Add other include directories as needed
# PUBLIC ${OtherLib_INCLUDE_DIRS}
)

# Add dependencies if needed
# add_dependencies(${PROJECT_NAME} ExternalDependency)
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,8 @@
*
****************************************************************************/


#include "MovingPlatformController.hpp"

using namespace custom;

#include <gz/plugin/Register.hh>

#include "gz/sim/components/Pose.hh"
Expand All @@ -47,6 +44,18 @@ using namespace custom;
#include <gz/math/Rand.hh>
#include <gz/math/Pose3.hh>


using namespace custom;

// Register the plugin
GZ_ADD_PLUGIN(
MovingPlatformController,
gz::sim::System,
gz::sim::ISystemPreUpdate,
gz::sim::ISystemPostUpdate,
gz::sim::ISystemConfigure
)

void MovingPlatformController::Configure(const gz::sim::Entity &entity,
const std::shared_ptr<const sdf::Element> &sdf,
gz::sim::EntityComponentManager &ecm,
Expand Down Expand Up @@ -142,7 +151,3 @@ void MovingPlatformController::sendVelocityCommands()
{

}

GZ_ADD_PLUGIN(MovingPlatformController, gz::sim::System, MovingPlatformController::ISystemPreUpdate)

GZ_ADD_PLUGIN_ALIAS(MovingPlatformController, "custom::MovingPlatformController")
Original file line number Diff line number Diff line change
Expand Up @@ -34,29 +34,35 @@
#pragma once

#include <gz/sim/System.hh>
#include <gz/transport/Node.hh>

namespace custom
{
class MovingPlatformController
: public gz::sim::System,
public gz::sim::ISystemConfigure,
public gz::sim::ISystemPreUpdate
class MovingPlatformController:
public gz::sim::System,
public gz::sim::ISystemPreUpdate,
public gz::sim::ISystemPostUpdate,
public gz::sim::ISystemConfigure
{
public:

MovingPlatformController();

void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) final;

void PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm) final;

void Configure(const gz::sim::Entity &entity,
const std::shared_ptr<const sdf::Element> &sdf,
gz::sim::EntityComponentManager &ecm,
gz::sim::EventManager &eventMgr) override;

MovingPlatformController();
~MovingPlatformController() override = default;

private:

gz::transport::Node _node;

void updatePose(const gz::sim::EntityComponentManager &ecm);
void updateVelocityCommands(const gz::math::Vector3d &mean_velocity);
void sendVelocityCommands();
Expand All @@ -72,6 +78,5 @@ class MovingPlatformController

gz::math::Vector3d _platform_position{0., 0., 0.};
gz::math::Quaterniond _platform_orientation{1., 0., 0., 0.};

};
} // end namespace custom
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
# Template Gazebo Plugin

This is a template for creating new Gazebo plugins for PX4. Follow these steps to create your own plugin:

1. Copy this directory and rename it to your plugin name
2. Update the project name in CMakeLists.txt
3. Rename and implement the TemplateSystem.hpp/cpp files
4. Add your plugin to the top-level CMakeLists.txt in the gz_plugins directory:
```cmake
add_subdirectory(your_plugin_directory)
```
5. Add your plugin's target to the `px4_gz_plugins` target dependencies in the top-level CMakeLists.txt:
```cmake
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem YourPluginSystem)
```
6. Update the server.config file to load your plugin:
```xml
<plugin entity_name="*" entity_type="world" filename="libYourPluginSystem.so" name="custom::YourPluginSystem"/>
```

## Plugin Structure

This template follows the standard Gazebo plugin structure:

- `TemplateSystem.hpp/cpp`: The main plugin system class that is loaded by Gazebo
- CMakeLists.txt: Build configuration for this plugin

## Testing Your Plugin

After building, you can test your plugin by adding it to the server.config file and running a simulation.

0 comments on commit a9b6ffb

Please sign in to comment.