What will you learn in this post

• How to create a Gazebo plugin to generate dragging when a model moves.
• How to create a simple propulsion system plugin

List of resources used in this post

• The ROSject with the code used in this post.
• Git of Pan And Tilt Project, in case you don’t want to use the ROSject.
• The question on Gazebo Answers
• A live version of this post on YouTube: https://youtu.be/8VzyWskliUI

Start the provided ROSject

The first thing you need to do is to have a copy of the ROSject we mentioned above. Click on the link to get an automatic copy. You should now see a ROSject called BuoyancyBot on your list of ROSjects, something like the image below:

BuoyancyBot simulation on ROSDS

After clicking on the Open button to open the ROSject, you should have the environment like the one in the image below:

BuoyancyBot opened on ROSDS

Finding the code

After opening the ROSject, you can see the code on the ~/simulation_ws/src folder. There you will find the folders buoyancy_tests_pkg and spawn_robot_tools. It is worth mentioning that the plugin we created is called fluid_resitance.cpp, which has the content below. Bear in mind that you can find that code on the ROSject:

#include "ros/callback_queue.h"
#include "ros/ros.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include <functional>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <ignition/math/Vector3.hh>
#include <thread>

namespace gazebo {
class FluidResitance : public ModelPlugin {
public:
  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {

    if (_sdf->HasElement("fluid_resitanceTopicName")) {
      this->fluid_resitanceTopicName =
          _sdf->Get<std::string>("fluid_resitanceTopicName");
    } else {
      ROS_WARN("No Topic Given name given, setting default name %s",
               this->fluid_resitanceTopicName.c_str());
    }

    if (_sdf->HasElement("NameLinkToApplyResitance")) {
      this->NameLinkToApplyResitance =
          _sdf->Get<std::string>("NameLinkToApplyResitance");
    } else {
      ROS_WARN("No NameLinkToApplyResitance Given name given, setting default "
               "name %s",
               this->NameLinkToApplyResitance.c_str());
    }

    if (_sdf->HasElement("rate")) {
      this->rate = _sdf->Get<double>("rate");
    } else {
      ROS_WARN("No rate Given name given, setting default "
               "name %f",
               this->rate);
    }

    // Store the pointer to the model
    this->model = _parent;
    this->world = this->model->GetWorld();
    this->link_to_apply_resitance =
        this->model->GetLink(this->NameLinkToApplyResitance);

    // Listen to the update event. This event is broadcast every
    // simulation iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        std::bind(&FluidResitance::OnUpdate, this));

    // Create a topic name
    // std::string fluid_resitance_index_topicName = "/fluid_resitance_index";

    // Initialize ros, if it has not already bee initialized.
    if (!ros::isInitialized()) {
      int argc = 0;
      char **argv = NULL;
      ros::init(argc, argv, "model_mas_controler_rosnode",
                ros::init_options::NoSigintHandler);
    }

    // Create our ROS node. This acts in a similar manner to
    // the Gazebo node
    this->rosNode.reset(new ros::NodeHandle("model_mas_controler_rosnode"));

#if (GAZEBO_MAJOR_VERSION >= 8)
    this->last_time = this->world->SimTime().Float();
#else
    this->last_time = this->world->GetSimTime().Float();
#endif

    // Freq
    ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Float32>(
        this->fluid_resitanceTopicName, 1,
        boost::bind(&FluidResitance::OnRosMsg, this, _1), ros::VoidPtr(),
        &this->rosQueue);
    this->rosSub = this->rosNode->subscribe(so);

    // Spin up the queue helper thread.
    this->rosQueueThread =
        std::thread(std::bind(&FluidResitance::QueueThread, this));

    ROS_WARN("Loaded FluidResitance Plugin with parent...%s, With Fluid "
             "Resitance = %f "
             "Started ",
             this->model->GetName().c_str(), this->fluid_resitance_index);
  }

  // Called by the world update start event
public:
  void OnUpdate() {
    float period = 1.0 / this->rate;

    // Get simulator time
#if (GAZEBO_MAJOR_VERSION >= 8)
    float current_time = this->world->SimTime().Float();
#else
    float current_time = this->world->GetSimTime().Float();
#endif
    float dt = current_time - this->last_time;

    if (dt <= period){
        ROS_DEBUG(">>>>>>>>>>TimePassed = %f, TimePeriod =%f ",dt, period);
        return;
    }else{
        this->last_time = current_time;
        this->ApplyResitance();
    }


  }

public:
  void SetResitance(const double &_force) {
    this->fluid_resitance_index = _force;
    ROS_WARN("model_fluid_resitance_index changed >> %f",
             this->fluid_resitance_index);
  }

  void UpdateLinearVel() {
#if (GAZEBO_MAJOR_VERSION >= 8)
    this->now_lin_vel = this->model->RelativeLinearVel();
#else
    this->now_lin_vel = this->model->GetRelativeLinearVel();
#endif
  }

  void ApplyResitance() {

    this->UpdateLinearVel();

#if (GAZEBO_MAJOR_VERSION >= 8)
    ignition::math::Vector3d force, torque;
#else
    math::Vector3 force, torque;
#endif

    ROS_WARN("LinearSpeed = [%f,%f,%f] ",this->now_lin_vel.x, this->now_lin_vel.y, this->now_lin_vel.z);

    force.x = -1.0 * this->fluid_resitance_index * this->now_lin_vel.x;
    force.y = -1.0 * this->fluid_resitance_index * this->now_lin_vel.y;
    force.z = -1.0 * this->fluid_resitance_index * this->now_lin_vel.z;

    // Changing the mass
    this->link_to_apply_resitance->AddRelativeForce(force);
#if (GAZEBO_MAJOR_VERSION >= 8)
    this->link_to_apply_resitance->AddRelativeTorque(
        torque -
        this->link_to_apply_resitance->GetInertial()->CoG().Cross(force));
#else
    this->link_to_apply_resitance->AddRelativeTorque(
        torque -
        this->link_to_apply_resitance->GetInertial()->GetCoG().Cross(force));
#endif

    ROS_WARN("FluidResitanceApplying = [%f,%f,%f] ",force.x, force.y, force.z);
  }

public:
  void OnRosMsg(const std_msgs::Float32ConstPtr &_msg) {
    this->SetResitance(_msg->data);
  }

  /// \brief ROS helper function that processes messages
private:
  void QueueThread() {
    static const double timeout = 0.01;
    while (this->rosNode->ok()) {
      this->rosQueue.callAvailable(ros::WallDuration(timeout));
    }
  }

  // Pointer to the model
private:
  physics::ModelPtr model;

  // Pointer to the update event connection
private:
  event::ConnectionPtr updateConnection;

  // Mas of model
  double fluid_resitance_index = 1.0;

  /// \brief A node use for ROS transport
private:
  std::unique_ptr<ros::NodeHandle> rosNode;

  /// \brief A ROS subscriber
private:
  ros::Subscriber rosSub;
  /// \brief A ROS callbackqueue that helps process messages
private:
  ros::CallbackQueue rosQueue;
  /// \brief A thread the keeps running the rosQueue
private:
  std::thread rosQueueThread;

  /// \brief A ROS subscriber
private:
  physics::LinkPtr link_to_apply_resitance;

private:
  std::string fluid_resitanceTopicName = "fluid_resitance";

private:
  std::string NameLinkToApplyResitance = "base_link";

private:
#if (GAZEBO_MAJOR_VERSION >= 8)
  ignition::math::Vector3d now_lin_vel;
#else
  math::Vector3 now_lin_vel;
#endif

private:
  double rate = 1.0;

private:
  float last_time = 0.0;

private:
  /// \brief The parent World
  physics::WorldPtr world;
};

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(FluidResitance)
} // namespace gazebo

Compile the plugin

Once you have the ROSject, be it on ROSDS or on your own computer, you can easily compile it with:

cd ~/simulation_ws/

catkin_make

Everything should have compiled without any problems. If compiling in your own computer raises errors, please remember running the command source /opt/ros/kinetic/setup.bash before calling catkin_make .

Using the plugin

The plugin we compiled previously has to be loaded by gazebo. We do that on the file

~/simulation_ws/src/buoyancy_tests_pkg/buoyancy_tests_pkg/urdf/simple_floating_sphere_buoyancy_control.urdf, by using the instructions below:

<!-- Add plugin Air Resistance -->
<gazebo>
    <plugin name="fluid_resitance_plugin" filename="libfluid_resitance_plugin.so">
        <fluid_resitanceTopicName>/fluid_resitance</fluid_resitanceTopicName>
        <NameLinkToApplyResitance>simple_sphere_base_link</NameLinkToApplyResitance>
        <rate>2.0</rate>
    </plugin>
</gazebo>

Lights, Camera Action

With everything in place, we can now run the simulation.  To launch the Simple Fluid Machine, we launch the simulation that has the mass control, the fluid resistance and the propulsion plugins enabled:

roslaunch buoyancy_tests_pkg main_sphere_buoyancy_control.launch

You should get something like:

buoyancy_bot

We can now start the Propulsion Control. After running the command below, you have to keep pressing the direction you want to apply force:

rosrun buoyancy_tests_pkg keyboard_propulsion.py

propulsion

/fluid_resitance

rostopic pub /fluid_resitance std_msgs/Float32 "data: 10.0"

FluidResitance_X_Axis = -1* FluidResitance * LinearSpeed_X_Axis
FluidResitance_Y_Axis = -1* FluidResitance * LinearSpeed_Y_Axis
FluidResitance_Z_Axis = -1* FluidResitance * LinearSpeed_Z_Axis

Air Dragging in Gazebo on ROSDS

There is more to it but have a look at the HectorPluginsAerodinamics, in which this code is based upon.

Change the Mass or buoyancy

rostopic pub /model_mass std_msgs/Float32 "data: 0.0"

This mass you can find it in the URDF of the  model: buoyancy_tests_pkg/urdf/simple_floating_sphere_buoyancy_control.urdf

<inertial>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <mass value="7.23822947387" />
    <inertia ixx="0.00576" ixy="0.0" ixz="0.0" iyy="0.00576" iyz="0.0" izz="0.00576"/>
</inertial>

See the Full code

To see the full code, open the IDE by going to the top menu and select Tools->IDE

IDE – fluidresitance on ROSDS

You can also have a look at it from the git: https://bitbucket.org/theconstructcore/buoyancy_tests_pkg/src/master/

Bear in mind that this code is a simplification of the Hector plugins. It’s simply made easier to use. Check out the original source: https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/blob/kinetic-devel/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_aerodynamics.cpp

So, that is the post for today. Remember that we also have a live version of this post on our YouTube channel, as can be seen on the link below. If you liked the content of this post or video, please consider subscribing to our channel and pressing the bell for a new video every single day:

Related resources:

• Git of Pan And Tilt Project
• Q&A Gazebo answers
• Interested to Learn More? Try this course on URDF creation for Gazebo and ROS

#airdrag #fluidresitance #Simulation #Gazebo #Robot #rostutorials #Ubuntu

Post edited by Miguel, Yuhong and Ruben.