本文整理汇总了C++中sdf::ElementPtr::GetFirstElement方法的典型用法代码示例。如果您正苦于以下问题：C++ ElementPtr::GetFirstElement方法的具体用法？C++ ElementPtr::GetFirstElement怎么用？C++ ElementPtr::GetFirstElement使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sdf::ElementPtr的用法示例。
在下文中一共展示了ElementPtr::GetFirstElement方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: Load
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)
//.........这里部分代码省略.........