plugin for gazebo
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
Asked by Nachum on 2012-10-17 08:04:32 UTC
Comments
What compile errors are you getting? Please paste them here.
Asked by gerkey on 2013-01-11 19:22:57 UTC
thanks for the help but never mind i'm not working with those plugins any more :)
Asked by Nachum on 2013-02-27 07:52:24 UTC