diff --git a/examples/plugin/velocity_plugin/CMakeLists.txt b/examples/plugin/velocity_plugin/CMakeLists.txt new file mode 100644 index 0000000000..866b9119a0 --- /dev/null +++ b/examples/plugin/velocity_plugin/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR) + +find_package(gz-cmake4 REQUIRED) + +project(Plugin_Linear_Velocity) + +gz_find_package(gz-plugin3 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin3_VERSION_MAJOR}) + +gz_find_package(gz-sim9 REQUIRED) +set(GZ_SIM_VER ${gz-sim9_VERSION_MAJOR}) + +add_library(PluginLinearVelocity SHARED PluginLinearVelocity.cc) + +set_property(TARGET PluginLinearVelocity PROPERTY CXX_STANDARD 17) +target_link_libraries(PluginLinearVelocity + PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) diff --git a/examples/plugin/velocity_plugin/PluginLinearVelocity.cc b/examples/plugin/velocity_plugin/PluginLinearVelocity.cc new file mode 100644 index 0000000000..f6e25c5d9c --- /dev/null +++ b/examples/plugin/velocity_plugin/PluginLinearVelocity.cc @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2025 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 +#include +#include +#include +#include + +// This header is required to register plugins. +// The good practice suggests to recall it in the ".cc" file + +#include +#include + +// The next row recalls the plugin's header. + +#include "PluginLinearVelocity.hh" + +// This is required to register the plugin. +// NOTE: The interfaces shall match what's in the plugin's header (see line 42 to 44). + +GZ_ADD_PLUGIN( + plugin_velocity::PluginLinearVelocity, + gz::sim::System, + plugin_velocity::PluginLinearVelocity::ISystemConfigure, + plugin_velocity::PluginLinearVelocity::ISystemPreUpdate) + + +using namespace plugin_velocity; + +// In the next section, all the functions that we need are recalled. +// Since the plugin's header (see PluginLinearVelocity.hh file) is used as "function declaration", +// in the following section the core purpose of our plugin will be defined. + +// For this plugin I decided to use the "Configure function" to read the field in the .sdf file +// as e.g. the link_name field. Let's dive into the details, analyzing step-by-step the following code. + + +void plugin_velocity::PluginLinearVelocity::Configure(const gz::sim::Entity &_identity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) +{ + // STEP1) Read property from SDF. + // In this case, I call it "link_name" (see line 71 in the velocity_world_plugin.sdf file) + + auto linkName = _sdf->Get("link_name"); + + // STEP 2) Store the lName variable the value of the "link_name" + + this -> lName = linkName; // Do not forget to declare the lName variable in the plugin's header file (see line 61 in the PluginLinearVelocity.hh file)) + + // Store the _identity value using the "this ->". + + this -> entity = _identity; + + // Create the model object to access all the function using as attibute the "entity" variable. + // The function "Model()" enables to get some important details + + auto model = gz::sim::Model(entity); + + // Look for link entity using "LinkByName" into "model" defined above. + + this -> linkEntity = model.LinkByName(_ecm, linkName); +} + + +void PluginLinearVelocity::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + + // Define the linear velocity vector using. + + gz::math::Vector3d lVel(1,0,0); + + // Create an object using the "Link" class. + + gz::sim::Link link_n(linkEntity); + + // The next row is the main command wich applies the linear velocity vector "lVel" on the link entity. + // For this example we are applying the linear velocity on a simple box that we defined in the sdf file (see line 34 in the velocity_world_plugin.sdf file) + + link_n.SetLinearVelocity(_ecm,lVel); +} + +// end diff --git a/examples/plugin/velocity_plugin/PluginLinearVelocity.hh b/examples/plugin/velocity_plugin/PluginLinearVelocity.hh new file mode 100644 index 0000000000..4ec6508af8 --- /dev/null +++ b/examples/plugin/velocity_plugin/PluginLinearVelocity.hh @@ -0,0 +1,65 @@ +/* +Copyright 2025 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. +*/ + +#ifndef SYSTEM_PLUGIN_HELLOWORLD_HH_ +#define SYSTEM_PLUGIN_HELLOWORLD_HH_ + + +#include +#include +#include +#include + + +// When a plugin's header file is coded, it is a good practice to define a customized namespace. +// In this case we call it "plugin_plugin_velocity". + +namespace plugin_velocity +{ + + // Under our customized namespace we define and recall all the functions and variables that we need. + + // Since our objective is to: + // - Read property from the .sdf file + // - Send the LinearVelovity command to a link entity + + // Here we have to use "ISystemConfigure" and "ISystemPreUpdate". + + class PluginLinearVelocity: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate + { + // The PreUpdate callback must be imported by the plugins that are inheriting ISystemPreUpdate. + // Since its nature, the PreUpdate callback is called every simulation iteration before the physics updates the world. + // On the other hand the Configure callback is called only once. + + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr)override; + + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + private: + gz::sim::Entity linkEntity; + gz::sim::Entity entity; + std::string lName; + }; +} + +#endif diff --git a/examples/plugin/velocity_plugin/README.md b/examples/plugin/velocity_plugin/README.md new file mode 100644 index 0000000000..3daf2e82eb --- /dev/null +++ b/examples/plugin/velocity_plugin/README.md @@ -0,0 +1,39 @@ +# Velocity World + +This example shows how to set a velocity command on a link. +This example will set the velocity command on a stand-alone link but the same approach can be applied for more complex robots. + +In particular, in the world there is a box that is free to move on a ground plane. +The plugin presented will apply a linear speed of 1 m/s along the x-axis. + +## Build + +From the root of the `gz-sim` repository, do the following to build the example: + +~~~ +cd gz-sim/examples/plugins/velocity_plugin +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `PluginLinearVelocity` library under `build`. + +## Run + +The plugin must be attached to an entity to be loaded. +This is demonstrated in the `velocity_world_plugin.sdf`. + +Before starting Gazebo, we must make sure it can find the plugin by doing: + +~~~ +cd gz-sim/examples/plugins/velocity_plugin +export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/build +~~~ + +Then load the example world: + + gz sim -v 3 velocity_world_plugin.sdf + +Once Gazebo is opened, toggle the `play/pause` buttons to see the box moving along a linear path. \ No newline at end of file diff --git a/examples/plugin/velocity_plugin/velocity_world_plugin.sdf b/examples/plugin/velocity_plugin/velocity_world_plugin.sdf new file mode 100644 index 0000000000..16e7d7aad6 --- /dev/null +++ b/examples/plugin/velocity_plugin/velocity_world_plugin.sdf @@ -0,0 +1,76 @@ + + + + + 0 0 0 0 0 0 + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0.1 0 0 0 + false + + + + + + 1.14395 + + 0.095329 + 0 + 0 + 0.381317 + 0 + 0.476646 + + + + + + 0.5 0.5 0.2 + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + 0.5 0.5 0.2 + + + + + + box + + + + +