void Servo::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
// parse SDF Properries
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("torque")) {
torque = sdf->Get<double>("torque");
} else {
torque = 5;
}
gzmsg << "initializing servo: " << topic << " joint=" << joint->GetName()
<< " torque=" << torque << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &Servo::Callback, this);
// connect to the world update event
// this will call update every iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Servo::Update, this, _1));
}
void DCMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
// Parse SDF properties
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("multiplier")) {
multiplier = sdf->Get<double>("multiplier");
} else {
multiplier = 1;
}
gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
<< " multiplier=" << multiplier << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &DCMotor::Callback, this);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&DCMotor::Update, this, _1));
}
virtual void Load( gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
// Start up ROS
int argc = 0;
ros::init( argc, NULL, "pendulum_robot" );
//ros::init( argc, NULL, "pendulum" );
this->model = _model;
this->world = _model->GetWorld();
this->joints = this->model->GetJoints();
// ROS Nodehandle
//this->node = new ros::NodeHandle( "~" );
this->node = new ros::NodeHandle( "pendulum" );
// ROS Subscriber
//this->sub = this->node->subscribe<pendulum::command>( "ros_pendulum_standup_controller", 1, &ros_pendulum_controller_c::ros_callback, this );
//this->client = this->node->serviceClient<pendulum::command>( "ros_pendulum_standup_controller" );
this->client = this->node->serviceClient<pendulum::rpc_command>( "pendulum_standup_command" );
/*
if( !client.isValid() ) {
std::cerr << "client not valid\n";
client.waitForExistence();
}
*/
//this->sub = this->node->subscribe<pendulum::command>( "ros_pendulum_standup_controller_state", 1, ros_pendulum_controller_c::request_state )
//this->server = this->node->advertiseService( "ros_pendulum_standup_controller_command", &ros_pendulum_controller_c::issue_command );
start_time = world->GetSimTime();
last_time = start_time;
this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind( &ros_pendulum_controller_c::Update, this ) );
std::cerr << "Pendulum Loaded" << std::endl;
}