plugin for gazebo [closed]

edit

Hi I am trying to write a PID controller for an inverted pendlum that i made in a URDF file. the controller is-

    #include <algorithm>
#include <assert.h>
#include "pendelum_controller/walk.h"
#include "PendelumController.h"
using namespace std;
namespace gazebo
{

////////////////////////////////////////////////////////////////////////////////
// Constructor
PendelumController::PendelumController()
{
    up=true;
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
PendelumController::~PendelumController()
{
  event::Events::DisconnectWorldUpdateStart(this->update_connection_);

  // Custom Callback Queue
  this->queue_.clear();
  this->queue_.disable();
  this->rosnode_->shutdown();
  this->callback_queue_thread_.join();

  delete this->rosnode_;
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void PendelumController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // Get the world name.
  this->world_ = _model->GetWorld();

  // load parameters
  this->robot_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";

  if (_sdf->HasElement("joint1")){
    this->only_joint_ =_model->GetJoint(_sdf->GetElement("joint1")->GetValueString());
  }else{
      ROS_FATAL("while loading gazebo_ros_force plugin,no joint1!\n");
  }
  // initialize ros if not done so already
  if (!ros::isInitialized())
  {
    ROS_FATAL("while loading gazebo_ros_force plugin, ros is not initialized, please load a gazebo system plugin that initializes ros (e.g. libgazebo_ros_api_plugin.so from gazebo ros package)\n");
    return;
  }

  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);

  // Custom Callback Queue
  /*ros::SubscribeOptions so = ros::SubscribeOptions::create<pendelum_controller::walk>(
    this->topic_name_,1,
    boost::bind( &PendelumController::UpdateObjectForce,this,_1),
    ros::VoidPtr(), &this->queue_);
  this->sub_ = this->rosnode_->subscribe(so);
*/
  // Custom Callback Queue
  this->callback_queue_thread_ = boost::thread( boost::bind( &PendelumController::QueueThread,this ) );

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateStart(
      boost::bind(&PendelumController::UpdateChild, this));
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void PendelumController::UpdateObjectForce(const pendelum_controller::walk::ConstPtr& _msg)
{
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void PendelumController::UpdateChild()
{
  this->lock_.lock();
 // ROS_INFO("degree [%f]",L_Knee_joint_->GetAngle(0).GetAsDegree());
          //if(up){
                    /*   float64 desired_pos =0.2;   
              float64 pos=this->only_joint_->GetAngle(0);
                          float64 vel=this->only_joint_->GetVelocity(0);*/

                        this->only_joint_->SetForce(0,-10*(this->only_joint_->GetAngle())-3*(this->only_joint_->GetVelocity()));
            /*  if(this->only_joint_->GetAngle(0)>this->only_joint_-> GetHighStop(0)/2){
                  this->only_joint_->SetForce(0,-10);
                  up=false;
              }
          }
          else{
              this->only_joint_->SetForce(0,-10);
              if(this->only_joint_->GetAngle(0)<this->only_joint_-> GetLowStop(0)/2){
                 this->only_joint_->SetForce(0,10);
                 up=true;
              }
          }*/
  this->lock_.unlock();
}

// Custom Callback Queue
////////////////////////////////////////////////////////////////////////////////
// custom callback queue thread
void PendelumController::QueueThread()
{
  static const double timeout = 0.01;

  while (this->rosnode_->ok())
  {
    this->queue_.callAvailable(ros::WallDuration(timeout));
  }
}

GZ_REGISTER_MODEL_PLUGIN(PendelumController);

}

I am trying to compile but getting errors. any idea? Is my syntax right?

Thanks