warthog_suspension_plugin.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "gazebo/gazebo.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/transport/transport.hh"
#include <boost/bind.hpp>
#include <string>

namespace gazebo
{
  /*
   * Define joint names.
   */
  static const std::string L_SUS_JNT = "left_diff_unit_joint";
  static const std::string R_SUS_JNT = "right_diff_unit_joint";

  class WarthogSuspensionPlugin : public ModelPlugin
  {
  public:
    WarthogSuspensionPlugin()
    {
      int argc = 0;
      char** argv = NULL;
      ros::init(argc, argv, "warthog_suspension_plugin");

      // Initialize last_update_time_
      last_update_time_ = ros::Time::now();
    }

    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
      // Save the pointer to the model
      this->model_ = _parent;

      // Load parameters
      sus_kf_ = _sdf->Get<double>("suspension_k");
      sus_bf_ = _sdf->Get<double>("suspension_b");

      // Retrieve the model joints
      suspension_jnts_["left"] = model_->GetJoint(L_SUS_JNT);
      suspension_jnts_["right"] = model_->GetJoint(R_SUS_JNT);

      // Listen for update event (every simulation iteration/step)
      this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
          boost::bind(&WarthogSuspensionPlugin::OnUpdate, this, _1) );
    }

    void OnUpdate(const common::UpdateInfo& _info)
    {
      // Calculate time interval
      ros::Time curr_time = ros::Time::now();
      double elapsed_t = (curr_time - last_update_time_).toSec();
      last_update_time_ = curr_time;

      // Apply the suspension forces based on current vehicle state
      applySuspension(sus_kf_, sus_bf_);
    }

  private:
  void applySuspension(double kf, double bf)
  {
    // Get the displacement and velocity of the left and right suspensions
    double l_d = suspension_jnts_["left"]->GetAngle(0).Radian();
    double r_d = suspension_jnts_["right"]->GetAngle(0).Radian();

    double l_v = suspension_jnts_["left"]->GetVelocity(0);
    double r_v = suspension_jnts_["right"]->GetVelocity(0);

    // Set spring and damper forces on each suspension
    suspension_jnts_["left"]->SetForce(0, -1*(kf*l_d)-(bf*l_v) );
    suspension_jnts_["right"]->SetForce(0, -1*(kf*r_d)-(bf*r_v) );
  }

  private:
    std::map<std::string, physics::JointPtr> suspension_jnts_;
    double sus_kf_, sus_bf_;
    ros::Time last_update_time_;
    physics::ModelPtr model_;
    event::ConnectionPtr updateConnection_;
  };

  // Register plugin
  GZ_REGISTER_MODEL_PLUGIN(WarthogSuspensionPlugin);
};  // namespace gazebo