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"]->Position();
    double r_d = suspension_jnts_["right"]->Position();
 
    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