gazebo::physics::LinkPtr WorldProxy::HELPER_getLinkInModel(gazebo::physics::ModelPtr model, std::string link_name)
{
gazebo::physics::Link_V model_links = model->GetLinks();
for(int i=0; i < model_links.size(); i++ )
{
std::string candidate_link = model_links[i]->GetScopedName();
if( HELPER_hasEnding(candidate_link,"::"+link_name) )
{
return model_links[i];
}
}
return gazebo::physics::LinkPtr();
}
//Here we already have the model!
bool robotSim::gazeboConfigureHook(gazebo::physics::ModelPtr model) {
if (model.get() == NULL) {
RTT::log(RTT::Error) << "No model could be loaded" << RTT::endlog();
return false;
}
if (!_models_loaded) {
RTT::log(RTT::Error) << "URDF and SRDF models has not been passed. Call loadURDFAndSRDF(URDF_path, SRDF_path)" << RTT::endlog();
return false;
}
// Get the joints
gazebo_joints_ = model->GetJoints();
model_links_ = model->GetLinks();
RTT::log(RTT::Info) << "Model name "<< model->GetName() << RTT::endlog();
RTT::log(RTT::Info) << "Model has " << gazebo_joints_.size() << " joints" << RTT::endlog();
RTT::log(RTT::Info) << "Model has " << model_links_.size() << " links" << RTT::endlog();
for(unsigned int i = 0; i < _xbotcore_model.get_chain_names().size(); ++i){
std::string chain_name = _xbotcore_model.get_chain_names()[i];
std::vector<std::string> enabled_joints_in_chain;
_xbotcore_model.get_enabled_joints_in_chain(chain_name,enabled_joints_in_chain);
kinematic_chains.insert(std::pair<std::string, boost::shared_ptr<KinematicChain>>(
chain_name, boost::shared_ptr<KinematicChain>(
new KinematicChain(chain_name, enabled_joints_in_chain, *(this->ports()), model
#ifdef USE_INTROSPECTION
,this
#endif
))));
}
RTT::log(RTT::Info) << "Kinematic Chains map created!" << RTT::endlog();
for(std::map<std::string, boost::shared_ptr<KinematicChain>>::iterator it = kinematic_chains.begin();
it != kinematic_chains.end(); it++){
if(!(it->second->initKinematicChain(gains.Gains))){
RTT::log(RTT::Warning) << "Problem Init Kinematic Chain" <<
it->second->getKinematicChainName() << RTT::endlog();
return false;
}
}
RTT::log(RTT::Info) << "Kinematic Chains Initialized!" << RTT::endlog();
RTT::log(RTT::Info) << "Checking Sensors (URDF/SRDF)"<<RTT::endlog();
std::map<std::string,int> ft_srdf = _xbotcore_model.get_ft_sensors();
std::map<std::string,int> imu_srdf = _xbotcore_model.get_imu_sensors();
RTT::log(RTT::Info) << "Force Update of SensorManager (before accessing sensors)!" << RTT::endlog();
gazebo::sensors::SensorManager::Instance()->Update(true);
RTT::log(RTT::Info) << "Checking Sensors (Gazebo)"<<RTT::endlog();
gazebo::sensors::Sensor_V sensors = gazebo::sensors::SensorManager::Instance()->GetSensors();
gazebo::sensors::Sensor_V sensors_attached_to_robot;
for(unsigned int i = 0; i < sensors.size(); ++i){
if(sensors[i]->ScopedName().find("::"+model->GetName()+"::") != std::string::npos)
sensors_attached_to_robot.push_back(sensors[i]);
}
for(std::map<std::string,int>::iterator i = ft_srdf.begin(); i != ft_srdf.end(); i++)
{
force_torque_sensor ft(i->first, model, _xbotcore_model.get_urdf_model(),
sensors_attached_to_robot,
*(this->ports())
#ifdef USE_INTROSPECTION
,this
#endif
);
if(ft.isInited())
force_torque_sensors.push_back(ft);
}
for(std::map<std::string,int>::iterator i = imu_srdf.begin(); i != imu_srdf.end(); ++i){
imu_sensor imu_(i->first,sensors_attached_to_robot,*(this->ports()));
if(imu_.isInited())
imu_sensors.push_back(imu_);
}
RTT::log(RTT::Warning) << "Done Configuring Component" << RTT::endlog();
return true;
}
