plugin for gazebo [closed]
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
What compile errors are you getting? Please paste them here.
thanks for the help but never mind i'm not working with those plugins any more :)