RTC::ReturnCode_t Simulation::onExecute(RTC::UniqueId ec_id)
{
if (m_p_dataIn.isNew())
{
std::cout << "DataIn New" << std::endl;
std::vector<float> extracted;
m_p_dataIn.read();
{
std::string port_str = (char*)m_p_data.data;
m_p_feedback.data = port_str.c_str();
m_p_feedbackOut.write();
Json::Value root;
Json::Reader reader;
bool parsedSuccess = reader.parse(port_str, root, false);
if(!parsedSuccess) {
//panic out
}
extracted = extractFloats(root["data"].asString(), "<WRIST>([ANGLE.1:%][ANGLE.2:%][ANGLE.3:%][ANGLE.4:%][ANGLE.5:%][ANGLE.6:%][GRIPPER:%][MOTOR_L:%][MOTOR_R:%])", '%');
int joint_handles[6] = {58, 60, 62, 64, 66, 68};
int joint_setModes[6] = {0, 0, 0, 0, 0, 0};
float joint_values[6];
for(int i = 0; i < 6; i++)
joint_values[i] = extracted[i];
simRosSetJointState(6, joint_handles, joint_setModes, joint_values);
}
#ifdef DEBUG
// std::cout << rosbridge_msg.toStyledString() << std::endl;
#endif
{
int joint_handles[2] = {50, 52};
int joint_setModes[2] = {2, 2};
float joint_values[2] = {extracted[7], extracted[8]};
simRosSetJointState(2, joint_handles, joint_setModes, joint_values);
}
#ifdef DEBUG
// std::cout << rosbridge_msg.toStyledString() << std::endl;
#endif
#ifdef DEBUG
// std::cout << rosbridge_msg.toStyledString() << std::endl;
#endif
}
coil::usleep(1000);
return RTC::RTC_OK;
}
//! (static)
std::vector<float> toFloats(const std::string & s){
std::deque<float> values;
extractFloats(s, values);
return std::vector<float>(values.begin(), values.end());
}
//------------------------------------------------------------------------------//
void ShaderTranslator::translateShaderParam(ScriptNodePtr& pScriptNode, ShaderParamsPtr& params)
{
if(pScriptNode->name == "auto_named_param")
{
String name;
StringKeyValueMap::iterator it = pScriptNode->keyValueMap.find("name");
if (it == pScriptNode->keyValueMap.end())
{
TITAN_EXCEPT_ITEMLOST(
"auto_named_param does not have a name "
);
return ;
}
name = it->second;
it = pScriptNode->keyValueMap.find("annotation");
if (it == pScriptNode->keyValueMap.end())
{
TITAN_EXCEPT_ITEMLOST(
"auto_named_param does not have a annotation "
);
return ;
}
const ShaderParams::AutoConstantDef* def = ShaderParams::getAutoConstantDef(it->second);
if(!def)
{
TITAN_EXCEPT_ITEMLOST(
"we do not have this type of auto_named_param: " + it->second
);
}
if(def->edType == ShaderParams::EDT_NONE)
params->setNamedAutoConstant(name, def->constantType);
else
{
it = pScriptNode->keyValueMap.find("extra_data");
if (it == pScriptNode->keyValueMap.end())
{
TITAN_EXCEPT_ITEMLOST(
"this auto_named_param need extra_data "
);
return ;
}
if(def->edType == ShaderParams::EDT_INT)
{
size_t extraData = StringConverter::parseUint(it->second);
params->setNamedAutoConstant(name, def->constantType, extraData);
}
else
{
float extraData = StringConverter::parseFloat(it->second);
params->setNamedAutoConstant(name, def->constantType, extraData);
}
}
}
else if(pScriptNode->name == "named_param")
{
String name, type;
StringKeyValueMap::iterator it = pScriptNode->keyValueMap.find("name");
if (it == pScriptNode->keyValueMap.end())
{
TITAN_EXCEPT_ITEMLOST(
"auto_named_param does not have a name "
);
return ;
}
name = it->second;
it = pScriptNode->keyValueMap.find("type");
if (it == pScriptNode->keyValueMap.end())
{
TITAN_EXCEPT_ITEMLOST(
"auto_named_param does not have type "
);
return ;
}
type = it->second;
uint count = 0;
bool isFLoat = false;
if(type.find("float") != String::npos)
{
isFLoat = true;
if (type.size() >= 6)
{
count = StringConverter::parseInt(type.substr(5));
}
else
count = 1;
}
else if(type.find("int") != String::npos)
{
isFLoat = false;
if (type.size() >= 4)
count = StringConverter::parseInt(type.substr(3));
else
count = 1;
}
int roundedCount = count%4 != 0 ? count + 4 - (count%4) : count;
bool hasVal = true;
it = pScriptNode->keyValueMap.find("val");
if (it == pScriptNode->keyValueMap.end())
{
hasVal = false;
}
String val = it->second;
if(isFLoat)
{
float* pFloat = TITAN_ALLOC_T(float, roundedCount, MEMCATEGORY_GENERAL);
extractFloats(val, pFloat, roundedCount);
params->setNamedConstant(name,pFloat, roundedCount, 1);
TITAN_FREE(pFloat, MEMCATEGORY_GENERAL);
}
else
{
