////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboPanel::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
gzwarn << "The GazeboPanel plugin is DEPRECATED in ROS hydro." << std::endl;
model_ = _model;
world_ = _model->GetWorld();
link_ = _model->GetLink();
link_name_ = link_->GetName();
namespace_.clear();
terminated_ = false;
// load parameters from sdf
if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue())
{
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link_ = _model->GetLink(link_name_);
}
if (_sdf->HasElement("jointName") && _sdf->GetElement("jointName")->GetValue())
{
std::string joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
joint_ = _model->GetJoint(joint_name_);
}
else
{
joint_ = _model->GetJoint("stem_joint");
}
if (!link)
{
ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
node_handle_ = new ros::NodeHandle(namespace_);
pub_score_ = node_handle_->advertise<std_msgs::String>("score", 1, true); // set latch true
pub_time_ = node_handle_->advertise<std_msgs::String>("remaining_time", 1);
ros::NodeHandle param_handle(*node_handle_, "controller");
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboPanel::Update, this));
}
void AuRo666Plugin::Load(physics::ModelPtr model_ptr, sdf::ElementPtr sdf_element_ptr) {
this->model_ptr = model_ptr;
initializePID(sdf_element_ptr);
/* ********************* Assigning the joint pointers *********************** */
// TODO: Replace such strings with const vars
joints[front_left_yaw_id] = model_ptr->GetJoint("front_left_joint");
joints[front_right_yaw_id] = model_ptr->GetJoint("front_right_joint");
joints[back_left_velocity_id] = model_ptr->GetJoint("back_left_joint");
joints[back_right_velocity_id] = model_ptr->GetJoint("back_right_joint");
last_update_time = model_ptr->GetWorld()->GetSimTime();
connection_ptr = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&AuRo666Plugin::updateState, this));
}
void PneumaticPiston::Load(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();
}
forward_force = sdf->Get<double>("forward-force");
reverse_force = sdf->Get<double>("reverse-force");
if (sdf->HasElement("direction") && sdf->Get<std::string>("direction") == "reversed") {
forward_force = -forward_force;
reverse_force = -reverse_force;
}
gzmsg << "Initializing piston: " << topic << " joint=" << joint->GetName()
<< " forward_force=" << forward_force
<< " reverse_force=" << reverse_force<< std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = transport::NodePtr(new transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &PneumaticPiston::Callback, this);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&PneumaticPiston::Update, this, _1));
}
void Servo::Load(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 = transport::NodePtr(new 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 = event::Events::ConnectWorldUpdateBegin(
boost::bind(&Servo::Update, this, _1));
}
void DCMotor::Load(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 = transport::NodePtr(new 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 = event::Events::ConnectWorldUpdateBegin(boost::bind(&DCMotor::Update, this, _1));
}
void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// 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("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
gzmsg << "Initializing potentiometer: " << topic << " joint=" << joint->GetName()
<< " radians=" << radians << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = transport::NodePtr(new transport::Node());
node->Init(scoped_name);
pub = node->Advertise<msgs::Float64>(topic);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Potentiometer::Update, this, _1));
}
bool findJointByName(physics::ModelPtr model, physics::JointPtr &joint, const std::string name) {
joint = model->GetJoint(name);
if (!joint) {
gzerr << "joint by name [" << name << "] not found in model\n";
return false;
}
return true;
}
void writeHeader(ofstream& file) {
file << "Time, ";
for (unsigned int i = 0; i < boost::size(joints); ++i) {
physics::JointPtr currJoint = model->GetJoint(joints[i]);
for (unsigned int j = 0; j < currJoint->GetAngleCount(); ++j) {
file << joints[i] << "(" << j << "),";
}
}
file << endl;
}
physics::JointPtr GazeboRos::getJoint(physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name) {
std::string joint_name;
getParameter<std::string>(joint_name, _tag_name, _joint_default_name);
physics::JointPtr joint = _parent->GetJoint(joint_name);
if (!joint)
{
char error[200];
snprintf(error, 200,
"%s: couldn't get wheel hinge joint named %s",
info() , joint_name.c_str());
gzthrow(error);
}
return joint;
}
InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf) {
joint = model->GetJoint(sdf->Get<std::string>("joint"));
min = sdf->Get<double>("min");
max = sdf->Get<double>("max");
if (sdf->HasElement("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
gzmsg << "\tinternal limit switch: " << " type=" << joint->GetName()
<< " range=[" << min << "," << max << "] radians=" << radians << std::endl;
}
private: void worldUpdate(){
if(this->model->GetWorld()->GetSimTime().nsec % 10000000 != 0){
return;
}
csvFile << world->GetSimTime().Float() << ", ";
// Iterate over model joints and print them
for (unsigned int i = 0; i < boost::size(joints); ++i) {
physics::JointPtr currJoint = model->GetJoint(joints[i]);
for(unsigned int j = 0; j < currJoint->GetAngleCount(); ++j){
csvFile << " " << currJoint->GetForce(j) << ", ";
}
}
csvFile << endl;
}
void Encoder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// 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("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
zero = GetAngle();
stopped = true;
stop_value = 0;
gzmsg << "Initializing encoder: " << topic << " joint=" << joint->GetName()
<< " radians=" << radians << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = transport::NodePtr(new transport::Node());
node->Init(scoped_name);
command_sub = node->Subscribe(topic + "/control", &Encoder::Callback, this);
pos_pub = node->Advertise<msgs::Float64>(topic + "/position");
vel_pub = node->Advertise<msgs::Float64>(topic + "/velocity");
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = event::Events::ConnectWorldUpdateBegin(
boost::bind(&Encoder::Update, this, _1));
}
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
model_ = parent;
if (!model_)
{
ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
return;
}
// Get then name of the parent model and use it as node name
std::string model_name = sdf->GetParent()->GetValueString("name");
gzdbg << "Plugin model name: " << model_name << "\n";
nh_ = ros::NodeHandle("");
// creating a private name pace until Gazebo implements topic remappings
nh_priv_ = ros::NodeHandle("/" + model_name);
node_name_ = model_name;
world_ = parent->GetWorld();
// TODO: use when implementing subs
// ros_spinner_thread_ = new boost::thread(boost::bind(&GazeboRosKobuki::spin, this));
//
// this->node_namespace_ = "";
// if (_sdf->HasElement("node_namespace"))
// this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";
/*
* Prepare receiving motor power commands
*/
motor_power_sub_ = nh_priv_.subscribe("commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
motors_enabled_ = true;
/*
* Prepare joint state publishing
*/
if (sdf->HasElement("left_wheel_joint_name"))
{
left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("right_wheel_joint_name"))
{
right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
if (!joints_[LEFT] || !joints_[RIGHT])
{
ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
return;
}
joint_state_.header.frame_id = "Joint States";
joint_state_.name.push_back(left_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_.name.push_back(right_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
/*
* Prepare publishing odometry data
*/
if (sdf->HasElement("wheel_separation"))
{
wheel_sep_ = sdf->GetElement("wheel_separation")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("wheel_diameter"))
{
wheel_diam_ = sdf->GetElement("wheel_diameter")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("torque"))
{
torque_ = sdf->GetElement("torque")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);
/*
* Prepare receiving velocity commands
*/
if (sdf->HasElement("velocity_command_timeout"))
{
cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
last_cmd_vel_time_ = world_->GetSimTime();
cmd_vel_sub_ = nh_priv_.subscribe("commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);
/*
* Prepare cliff sensors
*/
std::string cliff_sensor_left_name, cliff_sensor_front_name, cliff_sensor_right_name;
if (sdf->HasElement("cliff_sensor_left_name"))
{
cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_front_name"))
{
cliff_sensor_front_name = sdf->GetElement("cliff_sensor_front_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_right_name"))
{
cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
cliff_sensor_front_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_front_name));
cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
if (!cliff_sensor_left_)
{
ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_front_)
{
ROS_ERROR_STREAM("Couldn't find the frontal cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_right_)
{
ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_detection_threshold"))
{
cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_->SetActive(true);
cliff_sensor_front_->SetActive(true);
cliff_sensor_right_->SetActive(true);
cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>("events/cliff", 1);
/*
* Prepare bumper
*/
std::string bumper_name;
if (sdf->HasElement("bumper_name"))
{
bumper_name = sdf->GetElement("bumper_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
sensors::SensorManager::Instance()->GetSensor(bumper_name));
bumper_->SetActive(true);
bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>("events/bumper", 1);
prev_update_time_ = world_->GetSimTime();
ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
update_connection_ = event::Events::ConnectWorldUpdateStart(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
void GazeboJointStateClient::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Make sure the ROS node for Gazebo has already been initalized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' "
<< "in the gazebo_ros package)");
return;
}
// get joint names from parameters
std::string armNamespace = _parent->GetName();
if (_sdf->HasElement("robot_components_namespace"))
{
sdf::ElementPtr armParamElem = _sdf->GetElement("robot_components_namespace");
armNamespace = armParamElem->Get<std::string>();
}
else
{
ROS_WARN("SDF Element 'robot_components_namespace' not defined, so using robot name as namespace for components.");
}
// ROS_INFO_STREAM("GazeboJointStateClient loading joints from components namespace '"<<armNamespace<<"'");
ROS_INFO_STREAM("GazeboJointStateClient: Loading arm component parameters from "<< armNamespace);
joints = ArmComponentsNameManagerPtr(new ArmComponentsNameManager(armNamespace,false));
if (!joints->waitToLoadParameters(1, 3, 0.5))
{
ROS_FATAL_STREAM("Cannot load arm components for robot "<<_parent->GetName()<<" from namespace "<<armNamespace);
return;
}
physics::BasePtr jcChild = _parent->GetChild(physics::JointControllerThreadsafe::UniqueName());
if (!jcChild.get())
{
ROS_ERROR("Cannot load GazeboJointStateClient if no default JointControllerThreadsafe is set for the model");
throw std::string("Cannot load GazeboJointStateClient if no default JointController is set for the model");
}
physics::JointControllerThreadsafePtr ptr =
boost::dynamic_pointer_cast<physics::JointControllerThreadsafe>(jcChild);
if (!ptr.get())
{
ROS_ERROR_STREAM("Cannot load GazeboJointStateClient if child '"
<< physics::JointControllerThreadsafe::UniqueName()
<< "' is not of class JointControllerThreadsafe");
throw std::string("Cannot load GazeboJointStateClient if child '"
+ physics::JointControllerThreadsafe::UniqueName()
+ "' is not of class JointControllerThreadsafe");
}
jointController = ptr;
// get joint names from parameters
std::vector<std::string> joint_names;
joints->getJointNames(joint_names, true);
const std::vector<float>& arm_init = joints->getArmJointsInitPose();
const std::vector<float>& gripper_init = joints->getGripperJointsInitPose();
// Print the joint names to help debugging
std::map<std::string, physics::JointPtr > jntMap = jointController->GetJoints();
for (std::map<std::string, physics::JointPtr>::iterator it = jntMap.begin(); it != jntMap.end(); ++it)
{
physics::JointPtr j = it->second;
ROS_INFO_STREAM("Gazebo joint: '"<<j->GetName()<<"' is registered as '"<<it->first<<"'");
}
// check if the joint names maintained in 'joints' match the names in gazebo,
// that the joints can be used by this class, and if yes, load PID controllers.
int i = 0;
for (std::vector<std::string>::iterator it = joint_names.begin();
it != joint_names.end(); ++it)
{
// ROS_INFO_STREAM("Local joint name: '"<<*it<<"'");
physics::JointPtr joint = _parent->GetJoint(*it);
if (!joint.get())
{
ROS_FATAL_STREAM("Joint name " << *it << " not found as robot joint");
throw std::string("Joint not found");
}
std::string scopedName = joint->GetScopedName();
std::map<std::string, physics::JointPtr >::iterator jit = jntMap.find(scopedName);
if (jit == jntMap.end())
{
ROS_ERROR_STREAM("Joint name " << *it << " not found in joint controller joints");
throw std::string("Joint not found");
}
++i;
}
model = _parent;
jsSub = nh.subscribe(jointStateTopic, 1, &GazeboJointStateClient::JointStateCallback, this);
}
// Load the controller
void ServoPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
// Get the world name.
world = _model->GetWorld();
// default parameters
topicName = "drive";
jointStateName = "joint_states";
robotNamespace.clear();
controlPeriod = 0;
proportionalControllerGain = 8.0;
derivativeControllerGain = 0.0;
maximumVelocity = 0.0;
maximumTorque = 0.0;
// load parameters
if (_sdf->HasElement("robotNamespace")) robotNamespace = _sdf->Get<std::string>("robotNamespace");
if (_sdf->HasElement("topicName")) topicName = _sdf->Get<std::string>("topicName");
if (_sdf->HasElement("jointStateName")) jointStateName = _sdf->Get<std::string>("jointStateName");
if (_sdf->HasElement("firstServoName")) servo[FIRST].name = _sdf->Get<std::string>("firstServoName");
if (_sdf->HasElement("firstServoAxis")) servo[FIRST].axis = _sdf->Get<math::Vector3>("firstServoAxis");
if (_sdf->HasElement("secondServoName")) servo[SECOND].name = _sdf->Get<std::string>("secondServoName");
if (_sdf->HasElement("secondServoAxis")) servo[SECOND].axis = _sdf->Get<math::Vector3>("secondServoAxis");
if (_sdf->HasElement("thirdServoName")) servo[THIRD].name = _sdf->Get<std::string>("thirdServoName");
if (_sdf->HasElement("thirdServoAxis")) servo[THIRD].axis = _sdf->Get<math::Vector3>("thirdServoAxis");
if (_sdf->HasElement("proportionalControllerGain")) proportionalControllerGain = _sdf->Get<double>("proportionalControllerGain");
if (_sdf->HasElement("derivativeControllerGain")) derivativeControllerGain = _sdf->Get<double>("derivativeControllerGain");
if (_sdf->HasElement("maxVelocity")) maximumVelocity = _sdf->Get<double>("maxVelocity");
if (_sdf->HasElement("torque")) maximumTorque = _sdf->Get<double>("torque");
double controlRate = 0.0;
if (_sdf->HasElement("controlRate")) controlRate = _sdf->Get<double>("controlRate");
controlPeriod = controlRate > 0.0 ? 1.0/controlRate : 0.0;
servo[FIRST].joint = _model->GetJoint(servo[FIRST].name);
servo[SECOND].joint = _model->GetJoint(servo[SECOND].name);
servo[THIRD].joint = _model->GetJoint(servo[THIRD].name);
if (!servo[FIRST].joint)
gzthrow("The controller couldn't get first joint");
countOfServos = 1;
if (servo[SECOND].joint) {
countOfServos = 2;
if (servo[THIRD].joint) {
countOfServos = 3;
}
}
else {
if (servo[THIRD].joint) {
gzthrow("The controller couldn't get second joint, but third joint was loaded");
}
}
if (!ros::isInitialized()) {
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
}
rosnode_ = new ros::NodeHandle(robotNamespace);
transform_listener_ = new tf::TransformListener();
transform_listener_->setExtrapolationLimit(ros::Duration(1.0));
if (!topicName.empty()) {
ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::QuaternionStamped>(topicName, 1,
boost::bind(&ServoPlugin::cmdCallback, this, _1),
ros::VoidPtr(), &queue_);
sub_ = rosnode_->subscribe(so);
}
if (!jointStateName.empty()) {
jointStatePub_ = rosnode_->advertise<sensor_msgs::JointState>(jointStateName, 10);
}
joint_state.header.frame_id = transform_listener_->resolve(_model->GetLink()->GetName());
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&ServoPlugin::Update, this));
}
