plugin for gazebo [closed]

asked 2012-10-17 08:04:32 -0500

Nachum gravatar image

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

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by nkoenig
close date 2013-03-06 17:48:03.408462

Comments

What compile errors are you getting? Please paste them here.

gerkey gravatar imagegerkey ( 2013-01-11 18:22:57 -0500 )edit

thanks for the help but never mind i'm not working with those plugins any more :)

Nachum gravatar imageNachum ( 2013-02-27 06:52:24 -0500 )edit