void FreeFloatingControlPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
// get model and name
model_ = _model;
robot_namespace_ = model_->GetName();
controller_is_running_ = true;
// register ROS node & time
rosnode_ = ros::NodeHandle(robot_namespace_);
ros::NodeHandle control_node(rosnode_, "controllers");
t_prev_= 0;
// check for body or joint param
control_body_ = false;
control_joints_ = false;
while(!(control_body_ || control_joints_))
{
control_body_ = control_node.hasParam("config/body");
control_joints_ = control_node.hasParam("config/joints");
}
// *** SET UP BODY CONTROL
char param[FILENAME_MAX];
std::string body_command_topic, body_state_topic;
unsigned int i,j;
if(control_body_)
{
control_node.param("config/body/command", body_command_topic, std::string("body_command"));
control_node.param("config/body/state", body_state_topic, std::string("body_state"));
if(_sdf->HasElement("link"))
body_ = model_->GetLink(_sdf->Get<std::string>("link"));
else
body_ = model_->GetLinks()[0];
// parse thruster elements
std::vector<double> map_elem;
map_elem.clear();
double map_coef;
sdf::ElementPtr sdf_element = _sdf->GetFirstElement();
while(sdf_element)
{
if(sdf_element->GetName() == "thruster")
{
//register thruster only if map is present
if(sdf_element->HasElement("map"))
{
// register map coefs
std::stringstream ss(sdf_element->Get<std::string>("map"));
for(i=0; i<6; ++i)
{
ss >> map_coef;
map_elem.push_back(map_coef);
}
// register max effort
if(sdf_element->HasElement("effort"))
thruster_max_command_.push_back(sdf_element->Get<double>("effort"));
else
thruster_max_command_.push_back(100);
}
}
sdf_element = sdf_element->GetNextElement();
}
// build thruster map from map elements
thruster_nb_ = map_elem.size()/6;
if(thruster_nb_ != 0)
{
thruster_map_.resize(6, thruster_nb_);
for(i=0; i<6; ++i)
for(j=0; j<thruster_nb_; ++j)
thruster_map_(i,j) = map_elem[6*j + i];
ComputePseudoInverse(thruster_map_, thruster_inverse_map_);
}
body_command_.resize(6);
// initialize subscriber to body commands
ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Wrench>(
body_command_topic, 1,
boost::bind(&FreeFloatingControlPlugin::BodyCommandCallBack, this, _1),
ros::VoidPtr(), &callback_queue_);
body_command_subscriber_ = rosnode_.subscribe(ops);
body_command_received_ = false;
// push control data to parameter server
control_node.setParam("config/body/link", body_->GetName());
std::string axe[] = {"x", "y", "z", "roll", "pitch", "yaw"};
std::vector<std::string> controlled_axes;
if(thruster_nb_)
{
unsigned int thr_i, dir_i;
// compute max force in each direction, writes controlled axes
double thruster_max_effort;
for(dir_i=0; dir_i<6; ++dir_i)
{
thruster_max_effort = 0;
for(thr_i=0; thr_i<thruster_nb_; ++thr_i)
thruster_max_effort += thruster_max_command_[thr_i] * std::abs(thruster_map_(dir_i,thr_i));
if(thruster_max_effort != 0)
{
controlled_axes.push_back(axe[dir_i]);
// push to position PID
sprintf(param, "%s/position/i_clamp", axe[dir_i].c_str());
control_node.setParam(param, thruster_max_effort);
// push to velocity PID
sprintf(param, "%s/velocity/i_clamp", axe[dir_i].c_str());
control_node.setParam(param, thruster_max_effort);
}
}
// initialize publisher to thruster_use
thruster_use_.data.resize(thruster_max_command_.size());
thruster_use_publisher_ = rosnode_.advertise<std_msgs::Float32MultiArray>("thruster_use", 1);
}
else
{
// no thrusters? assume all axes are controlled
for(unsigned int i=0; i<6; ++i)
controlled_axes.push_back(axe[i]);
}
// push controlled axes
control_node.setParam("config/body/axes", controlled_axes);
}
