2
$\begingroup$

Question

Is it possible to modify or extend a pre-existing Gazebo Sim (Garden) plugin, such as the gz-sim-multicopter-control-system plugin to publish the quadcopter rotor velocities as Gazebo topics? How can this be done?

Summary

I am using Gazebo Garden (gz-sim7) to simulate a quadcopter. I have implemented two of Gazebo's pre-existing plugins for the quadcopter model flight. The plugins are gz-sim-multicopter-motor-model-system and gz-sim-multicopter-control-system, and their use in the model.sdf file is shown below. You can also find a more detailed version of the plugin implementation at multicopter_velocity_control.sdf.

<?xml version="1.0"?>
<sdf version="1.8">
    <model name="quadcopter">
        <!-- The links of the quadcopter model -->
        <!-- The links of the quadcopter model -->

        <!-- Multicopter Motor Model plugin for each rotor -->
        <!-- Rotor 0 -->
        <plugin
            filename="gz-sim-multicopter-motor-model-system"
            name="gz::sim::systems::MulticopterMotorModel">
            <robotNamespace>quadcopter</robotNamespace>
            <jointName>rotor_0_joint</jointName>
            <linkName>rotor_0</linkName>

            <!-- More Multicopter Motor Model plugin components -->

            <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
            <actuator_number>0</actuator_number>
            <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
            <motorType>velocity</motorType>
        </plugin>

        <!-- Similarly define Rotor 1, Rotor 2 and Rotor 3 -->

        <!-- Multicopter Control plugin -->
        <plugin
            filename="gz-sim-multicopter-control-system"
            name="gz::sim::systems::MulticopterVelocityControl">
            <robotNamespace>quadcopter</robotNamespace>
            <comLinkName>base_link</comLinkName>
            <commandSubTopic>gazebo/command/twist</commandSubTopic>
            <enableSubTopic>enable</enableSubTopic>

            <!-- More Multicopter Control plugin components -->

            <rotorConfiguration>
                <rotor>
                    <jointName>rotor_0_joint</jointName>

                    <!-- More Multicopter Control plugin rotor configuration components -->

                </rotor>

                <!-- Similarly configure Rotor 1, Rotor 2 and Rotor 3 -->

            </rotorConfiguration>
        </plugin>
    </model>
</sdf>

Is it possible to publish the rotor velocities using Gazebo topics? If not, is it possible to modify or extend the functionalities of the pre-existing plugins to publish the rotor velocities to topics after calculating them?

Further Info

I am using Gazebo Garden and ROS 2 Humble for this project. I have used the Gazebo's ros_gz_project_template as a starting framework for my project. There are four main packages in the workspace (my_package_application, my_package_bringup, my_package_description and my_package_gazebo), and the src directory looks something like this.

src
├── ros_gz
├── sdformat_urdf
├── my_package_application
├── my_package_bringup
├── my_package_description
│   ├── CMakeLists.txt
│   ├── hooks
│   ├── include
│   │   └── my_package_description
│   │       └── MotorVelocityPublisher.hh
│   ├── models
│   │   └──quadcopter
│   │       ├── model.config
│   │       └── model.sdf
│   ├── package.xml
│   └── src
│       └── MotorVelocityPublisher.cc
└── my_package_gazebo
    ├── CMakeLists.txt
    ├── hooks
    ├── include
    ├── package.xml
    ├── src
    └── worlds
        └── quadcopter.sdf

My initial idea was to create a custom plugin called MotorVelocityPublisher which would access the rotor velocity data from the gz-sim-multicopter-control-system but the plugin doesn't make the rotor velocity data easily accessible.

I have also thought about modifying or extending the existing gz-sim-multicopter-control-system plugin to publish the rotor velocity data. More specifically, creating a new RotorVelocitiesComponent.hh component to store the rotor velocity values.

// RotorVelocitiesComponent.hh

#ifndef ROTOR_VELOCITIES_COMPONENT_HH_
#define ROTOR_VELOCITIES_COMPONENT_HH_

#include <gz/sim/components/Component.hh>

namespace multicopter_control
{
    // Define a new Gazebo component to store rotor velocities
    GZ_SIM_DEFINE_COMPONENT(RotorVelocities, std::vector<double>)
}

#endif  // ROTOR_VELOCITIES_COMPONENT_HH_

Extending the PreUpdate method in the MulticopterVelocityControl.cc library to save the rotor velocity values into the component.

// Add the component header in MulticopterVelocityControl.cc
#include "RotorVelocitiesComponent.hh"

// Existing Multicopter Velocity Control methods

void MulticopterVelocityControl::PreUpdate(
    const UpdateInfo &_info,
    EntityComponentManager &_ecm)
{
    GZ_PROFILE("MulticopterVelocityControl::PreUpdate");

    // Existing code for the Multicopter Velocity Control Pre-Update

    this->velocityController->CalculateRotorVelocities(*frameData, cmdVel,
                                                       this->rotorVelocities);

    // New code to store the rotor velocities in the component
    auto rotorVelocitiesComp = _ecm.Component<multicopter_control::RotorVelocities>(this->model.Entity());
    if (!rotorVelocitiesComp)
    {
        _ecm.CreateComponent(this->model.Entity(), multicopter_control::RotorVelocities(this->rotorVelocities));
    }
    else
    {
        rotorVelocitiesComp->Data() = this->rotorVelocities;
    }   // New code ends here

    this->PublishRotorVelocities(_ecm, this->rotorVelocities);
}

The problem with this solution is that I don't know how to correctly save the extended gz-sim-multicopter-control-system plugin, build it, link it, etc. I'm not sure if this method will work or not.

$\endgroup$

2 Answers 2

1
$\begingroup$

My solution to this question is slightly different to the solutions I proposed in my question. Instead of creating a new RotorVelocitiesComponent.hh component to store the rotor velocity values, I modified the MulticopterVelocityControl.hh and MulticopterVelocityControl.cc files to advertise the rotor velocity data through a Gazebo topic.

Method

I copied the plugin source files into my package, similar to how it is done in Gazebo's ros_gz_project_template. My src directory now looks something like this.

src
├── ros_gz
├── sdformat_urdf
├── my_package_application
├── my_package_bringup
├── my_package_description
│   ├── CMakeLists.txt
│   ├── hooks
│   ├── include
│   │   └── my_package_description
│   │       ├── Common.hh
│   │       ├── LeeVelocityController.hh
│   │       ├── MulticopterVelocityControl.hh
│   │       └── Parameters.hh
│   ├── models
│   │   └──quadcopter
│   │       ├── model.config
│   │       └── model.sdf
│   ├── package.xml
│   └── src
│       ├── Common.cc
│       ├── LeeVelocityController.cc
│       └── MulticopterVelocityControl.cc
└── my_package_gazebo

The next step is to change the package CMakeLists.txt file to build the plugin correctly.

cmake_minimum_required(VERSION 3.8)
project(my_package_description)

# other ROS 2 CMakeLists.txt template code

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(Eigen3 REQUIRED)

# find the needed plugin dependencies
find_package(gz-cmake3 REQUIRED)
find_package(gz-plugin2 REQUIRED COMPONENTS register)
set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
find_package(gz-common5 REQUIRED COMPONENTS profiler)
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
find_package(gz-transport12 REQUIRED)
set(GZ_TRANSPORT_VER ${gz-transport12_VERSION_MAJOR})
find_package(gz-msgs9 REQUIRED)
set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR})

# set the right variables for gazebo garden
find_package(gz-sim7 REQUIRED)
set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Garden")

# define a library target named 'MyModifiedPlugin'.
add_library(MyModifiedPlugin
  SHARED
  src/MulticopterVelocityControl.cc
  src/Common.cc
  src/LeeVelocityController.cc
)

# specify 'include' as the include directory for compiling
# the 'MyModifiedPlugin' target.
target_include_directories(
  MyModifiedPlugin PRIVATE include
)

# specify the gazebo libraries needed when linking the 'MyModifiedPlugin' target.
target_link_libraries(MyModifiedPlugin PRIVATE
  gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER}
  gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER}
  gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
  gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
  Eigen3::Eigen
)

# copy the compiled libraries of 'MyModifiedPlugin' targets to the
# subfolder 'lib/my_package_description' of the install directory.
install(TARGETS MyModifiedPlugin
  DESTINATION lib/${PROJECT_NAME}
)

# other ROS 2 CMakeLists.txt template code
# any other custom CMakeLists.txt code

# I also use hooks to guide gazebo concerning where to look
# for resources and plugins

ament_package()

Now it's just a matter of correctly modifying the pre-existing plugin source files and correctly using the plugin in the model.sdf file.

Make sure to change any file inclusion directives to specify the correct locations of the header files. An example for this case would be changing the following inclusion directives

#include "Common.hh"
#include "LeeVelocityController.hh"
#include "MulticopterVelocityControl.hh"
#include "Parameters.hh"

to the following inclusion directives.

#include "my_package_description/Common.hh"
#include "my_package_description/LeeVelocityController.hh"
#include "my_package_description/MulticopterVelocityControl.hh"
#include "my_package_description/Parameters.hh"

The majority of plugin modifications were done in the MulticopterVelocityControl.hh and MulticopterVelocityControl.cc files. Useful inclusion directives were added to the MulticopterVelocityControl.hh.

// Pre-existing `MulticopterVelocityControl.hh` inclusion directives

#include <string>

// New msg header for rotor velocity variables
#include <gz/msgs/float_v.pb.h>

#include <gz/msgs/actuators.pb.h>

// Remaining pre-existing `MulticopterVelocityControl.hh` inclusion directives

Additional variables were added for the plugin modification.

namespace gz
{
namespace sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE
{
namespace systems
{
    class MulticopterVelocityControl
        :   public System,
            public ISystemConfigure,
            public ISystemPreUpdate
    {

        // Pre-existing `MulticopterVelocityControl.hh` declarations

        /// \brief Gazebo communication node.
        private: transport::Node node;

        /// \brief New topic for rotor velocities.
        private: std::string rotorVelocityPubTopic{"rotor_velocities"};

        /// \brief New rotor velocities publisher.
        private: transport::Node::Publisher rotorVelocityPub;

        /// \brief New variable that holds rotor velocities
        /// computed by the controller to later be published.
        private: gz::msgs::Float_V rotorVelocityPubMsg;

        // Remaining pre-existing `MulticopterVelocityControl.hh` declarations

    };
}
}
}
}

#endif

The rest of the changes were made in MulticopterVelocityControl.cc. First, add new logic to the MulticopterVelocityControl::Configure method to initialise the topic advertiser.

void MulticopterVelocityControl::Configure(const Entity &_entity,
                                           const std::shared_ptr<const sdf::Element> &_sdf,
                                           EntityComponentManager &_ecm,
                                           EventManager & /*_eventMgr*/)
{
  
    // Pre-existing Configure method logic

    sdfClone->Get<std::string>("enableSubTopic",
                                this->enableSubTopic, this->enableSubTopic);
    this->enableSubTopic = transport::TopicUtils::AsValidTopic(
        this->enableSubTopic);
    if (this->enableSubTopic.empty())
    {
        gzerr << "Invalid enable sub-topic." << std::endl;
        return;
    }

    // New code for rotor velocity publisher topic name
    sdfClone->Get<std::string>("rotorVelocityPubTopic",
                                this->rotorVelocityPubTopic, this->rotorVelocityPubTopic);
    this->rotorVelocityPubTopic = transport::TopicUtils::AsValidTopic(
        this->rotorVelocityPubTopic);
    if (this->rotorVelocityPubTopic.empty())
    {
        gzerr << "Invalid rotor velocity pub-topic." << std::endl;
        return;
    }   // New code ends here

    // Pre-existing code to subscribe to actuator command messages

    // New initialise the publisher for rotor velocities
    std::string rotorVelocityTopic{this->robotNamespace + "/" + this->rotorVelocityPubTopic};
    this->rotorVelocityPub = this->node.Advertise<gz::msgs::Float_V>(rotorVelocityTopic);
    gzmsg << "MulticopterVelocityControl publishing Double messages on ["
            << rotorVelocityTopic << "]" << std::endl;  // New code ends here

    // Remaining pre-existing Configure method logic

    this->initialized = true;
}

Finally, add additional logic to MulticopterVelocityControl.cc to publish the rotor velocities. I chose to do this in the MulticopterVelocityControl::PublishRotorVelocities function to prevent the simulation from breaking as much as possible.

void MulticopterVelocityControl::PublishRotorVelocities(
    EntityComponentManager &_ecm,
    const Eigen::VectorXd &_vels)
{
  
    // Pre-existing PublishRotorVelocities method logic

    // New code to clear previous data in the message
    // to prepare for new rotor velocities
    this->rotorVelocityPubMsg.clear_data();

    for (int i = 0; i < this->rotorVelocities.size(); ++i)
    {

        // Pre-existing logic to set rotorVelocitiesMsg velocities

        // New code to add each velocity value to the rotorVelocityPubMsg
        this->rotorVelocityPubMsg.add_data(_vels(i));
    }

    // New code to publish rotor velocities using rotorVelocityPub publisher
    this->rotorVelocityPub.Publish(this->rotorVelocityPubMsg);

    // Remaining pre-existing PublishRotorVelocities method logic

}

The new plugin implementation can be done by directly specifying the plugin .so file location, similarly as described in one of the answers to this question.

<plugin
    filename="libMyModifiedPlugin"
    name="gz::sim::v7::systems::MulticopterVelocityControl">

</plugin>

Gazebo can also easily find the plugin in the lib/my_package_description folder if you use hook files to widen Gazebo's search. The hook files will need to be specified in CMakeLists.txt. You can also expand Gazebo's search by setting the GZ_SIM_SYSTEM_PLUGIN_PATH environment variable using the command.

export GZ_SIM_SYSTEM_PLUGIN_PATH=${GZ_SIM_SYSTEM_PLUGIN_PATH}:<path-to-your-plugin-library>

So far the simulation has been running well, without breaking.

$\endgroup$
0
$\begingroup$

I think your proposed approach is the correct one. Simply copy the plugin source code folder to somewhere on your local machine and modify it as needed. To build it, run the following from the root of your plugin (where the CMakeLists.txt is).

mkdir build
cd build
cmake ..
make

Your compiled .so file can be found in the build folder. Before using the plugin you will need to tell Gazebo where to find it. This can be done by setting the appropriate env var, or using an absolute path, for example:

<plugin filename="/home/me/my_plugin/build/lib_my_plugin.so" name="MyPluginNameSpace::MyPluginClass">
</plugin>

Note this all assumes you have properly installed Gazebo and whatever dev packages are required. If you have a standard ROS2/Gazebo installation, it should work. Hope this helps.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.