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); }