void addVectorOfStringToProperty(yarp::os::Property& prop, std::string key, std::vector<std::string> & list)
{
prop.addGroup(key);
yarp::os::Bottle & bot = prop.findGroup(key).addList();
for(size_t i=0; i < list.size(); i++)
{
bot.addString(list[i].c_str());
}
return;
}
bool GazeboYarpJointSensorsDriver::setJointPointers(yarp::os::Property & pluginParameters) //WORKS
{
std::cout << ".ini file found, using joint names in ini file" << std::endl;
yarp::os::Bottle joint_names_bottle = pluginParameters.findGroup("jointNames");
if (joint_names_bottle.isNull()) {
std::cout << "GazeboYarpJointSensorsDriver::setJointPointers() error: cannot find jointNames parameter." << std::endl;
return false;
}
jointsensors_nr_of_channels = joint_names_bottle.size() - 1;
if (jointsensors_nr_of_channels == 0) {
std::cout << "GazeboYarpJointSensorsDriver::setJointPointers() error: no joint selected." << std::endl;
return false;
}
joint_ptrs.resize(jointsensors_nr_of_channels);
const gazebo::physics::Joint_V & gazebo_models_joints = _robot->GetJoints();
for(unsigned int i=0; i < joint_ptrs.size(); i++ ) {
bool joint_found = false;
std::string controlboard_joint_name(joint_names_bottle.get(i+1).asString().c_str());
std::string controlboard_joint_name_scoped_ending = "::" + controlboard_joint_name;
for(unsigned int gazebo_joint = 0; gazebo_joint < gazebo_models_joints.size() && !joint_found; gazebo_joint++ ) {
std::string gazebo_joint_name = gazebo_models_joints[gazebo_joint]->GetScopedName();
if( GazeboYarpPlugins::hasEnding(gazebo_joint_name,controlboard_joint_name_scoped_ending) ) {
joint_found = true;
joint_ptrs[i] = boost::get_pointer(gazebo_models_joints[gazebo_joint]);
}
}
if( !joint_found ) {
std::cout << "GazeboYarpJointSensorsDriver::setJointPointers(): Error, cannot find joint " << controlboard_joint_name << std::endl;
joint_ptrs.resize(0);
jointsensors_nr_of_channels = 0;
return false;
}
}
return true;
}
bool SkinGroup::initSensors(yarp::os::Property &config)
{
yarp::os::Bottle skinfiles = config.findGroup("parts").tail();
yarp::os::ConstString skinFilePath = config.check("skinsensorpath", yarp::os::Value("C:/roboskin/software_yarp_related/iCub/app/kaspar/conf"), "specifiy skinsensor config file path").asString();
// yarp::os::ConstString skinFilePath = config.check("skinsensorfilepath", yarp::os::Value("C:/roboskin/software_yarp_related/iCub/app/kaspar/conf"), "specifiy skinsensor config file path").asString();
printf("==== %s %d \n", skinFilePath.c_str(), skinfiles.size());
for(int i = 0; i < skinfiles.size(); i++)
{
yarp::os::Property skinproperty;
yarp::os::Bottle sensorConf(skinfiles.get(i).toString());
if(skinproperty.fromConfigFile((skinFilePath + FILESEPARATOR + sensorConf.get(0).asString())))
{
// put new attribute ifLog to the property before initialising skin sensors
//if(ifLogData)
//{
// skinproperty.put("logdata", ifLogData);
// skinproperty.put("logfilepath", logpath.c_str());
//}
SkinBodyPart *sk = new SkinBodyPart(skinproperty); // assuming property file is correct. No error checking and return. so be very careful
sk->setPortToConnect(sensorConf.get(1).asString());
if(sk->initConfiguration())
{
this->skinParts.push_back(sk);
}
else
{
delete sk;
return false;
}
}
else
{
fprintf(stderr, "Error loading skin sensor config file %s\n", sensorConf.get(0).asString().c_str());
return false;
}
}
return true;
}