ulAssetManager::ulAssetManager(std::string artFilePath)
{
// Should probably find a better way of doing this bit
int len = 256;
char pBuf[len];
char szTmp[32];
sprintf(szTmp, "/proc/%d/exe", getpid());
int bytes = std::min((int)readlink(szTmp, pBuf, len), len - 1);
if(bytes >= 0)
{
pBuf[bytes] = '\0';
}
progDir = std::string(pBuf);
progDir = progDir.substr(0, progDir.length() - 10);
std::ifstream artFile(progDir + artFilePath);
if (!artFile.is_open())
{
std::cerr<<"Could not open art definition file ("<<progDir + artFilePath<<")"<<std::endl;
}
else
{
std::string jsonString((std::istreambuf_iterator<char>(artFile)),
std::istreambuf_iterator<char>());
doc.Parse(jsonString.c_str());
assert(!doc.HasParseError());
}
}
void MyPinsFileIO::SaveAllRepositoryPinsToDisk(const std::vector<MyPinModel*>& pinModels)
{
if (m_fileIO.Exists(MyPinsDataFilename))
{
m_fileIO.DeleteFile(MyPinsDataFilename);
}
rapidjson::Document jsonDoc;
jsonDoc.SetObject();
rapidjson::Value pinsArray(rapidjson::kArrayType);
pinsArray.SetArray();
rapidjson::Document::AllocatorType& allocator = jsonDoc.GetAllocator();
jsonDoc.AddMember(MyPinsJsonArrayName.c_str(), pinsArray, allocator);
rapidjson::Value& myPinsArray = jsonDoc[MyPinsJsonArrayName.c_str()];
for (std::vector<MyPinModel*>::const_iterator it = pinModels.begin(); it != pinModels.end(); ++it)
{
const MyPinModel* pinModel = *it;
const Eegeo::Space::LatLong& latLong = pinModel->GetLatLong();
rapidjson::Value pinModelJson;
MyPinModelToJson(*pinModel, allocator, m_myPinBoundObjectRepository, pinModelJson);
myPinsArray.PushBack(pinModelJson, allocator);
}
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
jsonDoc.Accept(writer);
std::string jsonString(strbuf.GetString());
m_fileIO.WriteFile((Byte*)jsonString.c_str(), jsonString.size(), MyPinsDataFilename);
}
void SimpleJsonDB::flush()
{
qDebug() << "flush for db:" << path << "thread:" << QThread::currentThread();
// skip flush if delay is -1
if (maxFlushDelay == -1 || path == ":memory:")
{
qDebug() << "skipping flush for:" << jsonString();
return;
}
// stop pending timers
flushTimer.stop();
QString dbString = jsonString();
if (dbString == "{}")
{
// db is empty, remove old db file (if it exists)
QFile file(path);
file.remove();
}
else
{
dbString += '\n';
// save json file with atomic QSaveFile
QSaveFile file(path);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
{
qWarning() << "unable to write to db:" << path;
}
else
{
QTextStream outStream(&file);
outStream << dbString;
file.commit();
}
#if QT_VERSION >= 0x050500
qInfo() << "flushed db:" << path;
#endif
}
emit flushed();
}
QMap<QString, QVariant> JsonReader::decodeFile(QString filePath)
{
QFile file;
file.setFileName(filePath);
file.open(QIODevice::ReadOnly);
QByteArray byteArray = file.readAll();
file.close();
QString jsonString(byteArray);
Json json;
return json.decode(jsonString);
}
// Reads a file containing json text and stores that in our json variable to go through
// in our forge method
void SceneBuilder::jsonFromFile(std::string fileName)
{
std::ifstream t(fileName);
std::string jsonString((std::istreambuf_iterator<char>(t)),std::istreambuf_iterator<char>());
std::string errorString;
// Deferencing the pointer so we can use the overloaded operator method provided
// by our json11::Json method
this->setJson(new json11::Json(json11::Json::parse(jsonString, errorString)));
std::cout << "Error string: '" << errorString << "'" << std::endl;
};
void CreateEmptyJsonFile(Eegeo::Helpers::IFileIO& fileIO)
{
rapidjson::Document jsonDoc;
jsonDoc.SetObject();
rapidjson::Value myArray(rapidjson::kArrayType);
jsonDoc.AddMember(MyPinsJsonArrayName.c_str(), myArray, jsonDoc.GetAllocator());
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
jsonDoc.Accept(writer);
std::string jsonString(strbuf.GetString());
fileIO.WriteFile((Byte*)jsonString.c_str(), jsonString.size(), MyPinsDataFilename);
}
void CheckpointService::PIdLsnInfo::writeFile() {
util::LockGuard<util::Mutex> guard(mutex_);
std::string lsnInfoFileName;
util::NamedFile file;
try {
picojson::object jsonNodeInfo;
NodeAddress &address =
checkpointService_->partitionTable_->getNodeInfo(0)
.getNodeAddress();
jsonNodeInfo["address"] = picojson::value(address.toString(false));
jsonNodeInfo["port"] =
picojson::value(static_cast<double>(address.port_));
picojson::object jsonLsnInfo;
picojson::array lsnList;
for (PartitionId pId = 0; pId < partitionNum_; ++pId) {
lsnList.push_back(
picojson::value(static_cast<double>(lsnList_[pId])));
}
picojson::object jsonObject;
jsonObject["nodeInfo"] = picojson::value(jsonNodeInfo);
jsonObject["partitionNum"] =
picojson::value(static_cast<double>(partitionNum_));
jsonObject["groupNum"] =
picojson::value(static_cast<double>(partitionGroupNum_));
jsonObject["lsnInfo"] = picojson::value(lsnList);
std::string jsonString(picojson::value(jsonObject).serialize());
util::FileSystem::createPath(
path_.c_str(), PID_LSN_INFO_FILE_NAME.c_str(), lsnInfoFileName);
file.open(lsnInfoFileName.c_str(), util::FileFlag::TYPE_READ_WRITE |
util::FileFlag::TYPE_CREATE |
util::FileFlag::TYPE_TRUNCATE);
file.lock();
file.write(jsonString.c_str(), jsonString.length());
file.close();
}
catch (std::exception &e) {
GS_RETHROW_USER_ERROR(e, "Write lsn info file failed. (fileName="
<< lsnInfoFileName.c_str() << ", reason="
<< GS_EXCEPTION_MESSAGE(e) << ")");
}
}
QVariantMap APIHelper::jsonToVariantMap(const QByteArray &json)
{
#ifdef QT_DEBUG
qDebug() << "Start to parse JSON into QVariantMap.";
#endif
QString jsonString(json);
#ifdef QT_DEBUG
qDebug() << json;
#endif
QJsonParseError err;
QJsonDocument jsonDoc = QJsonDocument::fromJson(jsonString.toUtf8(), &err);
#ifdef QT_DEBUG
qDebug() << err.errorString();
#endif
return jsonDoc.object().toVariantMap();
}
std::string jsonString(BSONObj &obj, JsonStringFormat format, int pretty, UUIDEncoding uuidEncoding)
{
if ( obj.isEmpty() ) return "{}";
StringBuilder s;
s << "{";
BSONObjIterator i(obj);
BSONElement e = i.next();
if ( !e.eoo() )
while ( 1 ) {
if ( pretty ) {
s << '\n';
for( int x = 0; x < pretty; x++ )
s << " ";
}
else {
s << " ";
}
s << jsonString(e, format, true, pretty?pretty+1:0, uuidEncoding);
e = i.next();
if (e.eoo()) {
s << '\n';
for( int x = 0; x < pretty - 1; x++ )
s << " ";
s << "}";
break;
}
s << ",";
/* if ( e.eoo() )
break;
s << ","; */
}
//s << "\n}";
return s.str();
}
void processAndLogNewSpawnException(SpawnException &e, const Options &options,
const SpawningKit::ConfigPtr &config)
{
TRACE_POINT();
UnionStation::TransactionPtr transaction;
ErrorRenderer renderer(*config->resourceLocator);
string appMessage = e.getErrorPage();
string errorId;
char filename[PATH_MAX];
stringstream stream;
if (options.analytics && config->unionStationCore != NULL) {
try {
UPDATE_TRACE_POINT();
transaction = config->unionStationCore->newTransaction(
options.getAppGroupName(),
"exceptions",
options.unionStationKey);
errorId = transaction->getTxnId();
} catch (const tracable_exception &e2) {
transaction.reset();
P_WARN("Cannot log to Union Station: " << e2.what() <<
"\n Backtrace:\n" << e2.backtrace());
}
}
UPDATE_TRACE_POINT();
if (appMessage.empty()) {
appMessage = "none";
}
if (errorId.empty()) {
errorId = config->randomGenerator->generateHexString(4);
}
e.set("error_id", errorId);
try {
int fd = -1;
string errorPage;
UPDATE_TRACE_POINT();
errorPage = renderer.renderWithDetails(appMessage, options, &e);
#if (defined(__linux__) && (__GLIBC__ > 2 || (__GLIBC__ == 2 && __GLIBC_MINOR__ >= 11))) || defined(__APPLE__) || defined(__FreeBSD__)
snprintf(filename, PATH_MAX, "%s/passenger-error-XXXXXX.html",
getSystemTempDir());
fd = mkstemps(filename, sizeof(".html") - 1);
#else
snprintf(filename, PATH_MAX, "%s/passenger-error.XXXXXX",
getSystemTempDir());
fd = mkstemp(filename);
#endif
FdGuard guard(fd, NULL, 0, true);
if (fd == -1) {
int e = errno;
throw SystemException("Cannot generate a temporary filename",
e);
}
UPDATE_TRACE_POINT();
writeExact(fd, errorPage);
} catch (const SystemException &e2) {
filename[0] = '\0';
P_ERROR("Cannot render an error page: " << e2.what() << "\n" <<
e2.backtrace());
}
if (transaction != NULL) {
try {
UPDATE_TRACE_POINT();
transaction->message("Context: spawning");
transaction->message("Message: " +
jsonString(e.what()));
transaction->message("App message: " +
jsonString(appMessage));
const char *kind;
switch (e.getErrorKind()) {
case SpawnException::PRELOADER_STARTUP_ERROR:
kind = "PRELOADER_STARTUP_ERROR";
break;
case SpawnException::PRELOADER_STARTUP_PROTOCOL_ERROR:
kind = "PRELOADER_STARTUP_PROTOCOL_ERROR";
break;
case SpawnException::PRELOADER_STARTUP_TIMEOUT:
kind = "PRELOADER_STARTUP_TIMEOUT";
break;
case SpawnException::PRELOADER_STARTUP_EXPLAINABLE_ERROR:
kind = "PRELOADER_STARTUP_EXPLAINABLE_ERROR";
break;
case SpawnException::APP_STARTUP_ERROR:
kind = "APP_STARTUP_ERROR";
break;
case SpawnException::APP_STARTUP_PROTOCOL_ERROR:
kind = "APP_STARTUP_PROTOCOL_ERROR";
break;
case SpawnException::APP_STARTUP_TIMEOUT:
kind = "APP_STARTUP_TIMEOUT";
break;
case SpawnException::APP_STARTUP_EXPLAINABLE_ERROR:
kind = "APP_STARTUP_EXPLAINABLE_ERROR";
break;
default:
kind = "UNDEFINED_ERROR";
break;
}
transaction->message(string("Kind: ") + kind);
Json::Value details;
const map<string, string> &annotations = e.getAnnotations();
map<string, string>::const_iterator it, end = annotations.end();
for (it = annotations.begin(); it != end; it++) {
details[it->first] = it->second;
}
// This information is not very useful. Union Station
// already collects system metrics.
details.removeMember("system_metrics");
// Don't include environment variables because they may
// contain sensitive information.
details.removeMember("envvars");
transaction->message("Details: " + stringifyJson(details));
} catch (const tracable_exception &e2) {
P_WARN("Cannot log to Union Station: " << e2.what() <<
"\n Backtrace:\n" << e2.backtrace());
}
}
UPDATE_TRACE_POINT();
stream << "Could not spawn process for application " << options.appRoot <<
": " << e.what() << "\n" <<
" Error ID: " << errorId << "\n";
if (filename[0] != '\0') {
stream << " Error details saved to: " << filename << "\n";
}
stream << " Message from application: " << appMessage << "\n";
P_ERROR(stream.str());
if (config->agentsOptions != NULL) {
HookScriptOptions hOptions;
hOptions.name = "spawn_failed";
hOptions.spec = config->agentsOptions->get("hook_spawn_failed", false);
hOptions.agentsOptions = config->agentsOptions;
hOptions.environment.push_back(make_pair("PASSENGER_APP_ROOT", options.appRoot));
hOptions.environment.push_back(make_pair("PASSENGER_APP_GROUP_NAME", options.getAppGroupName()));
hOptions.environment.push_back(make_pair("PASSENGER_ERROR_MESSAGE", e.what()));
hOptions.environment.push_back(make_pair("PASSENGER_ERROR_ID", errorId));
hOptions.environment.push_back(make_pair("PASSENGER_APP_ERROR_MESSAGE", appMessage));
oxt::thread(boost::bind(runHookScripts, hOptions),
"Hook: spawn_failed", 256 * 1024);
}
}
int main(int argC,char **argV)
{
ros::init(argC,argV,"startBody");
ros::NodeHandle n;
std::string serverAddress;
n.getParam("/serverNameOrIP",serverAddress);
Socket mySocket(serverAddress.c_str(),"9003",streamSize);
iconv_t charConverter = iconv_open("UTF-8","UTF-16");
ros::Publisher bodyPub = n.advertise<k2_client::BodyArray>(topicName,1);
char jsonCharArray[readSkipSize];
while(ros::ok())
{
mySocket.readData();
char *jsonCharArrayPtr;
char *socketCharArrayPtr;
jsonCharArrayPtr = jsonCharArray;
socketCharArrayPtr = mySocket.mBuffer;
iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize);
double utcTime;
memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double));
std::string jsonString(jsonCharArray);
//std::cout<<jsonCharArray<<std::endl<<"***"<<std::endl;
Json::Value jsonObject;
Json::Reader jsonReader;
bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
if(!parsingSuccessful)
{
std::cout<<"Failure to parse: "<<parsingSuccessful<<std::endl;
continue;
}
k2_client::BodyArray bodyArray;
try
{
for(int i=0;i<6;i++)
{
k2_client::Body body;
body.header.stamp = ros::Time(utcTime);
body.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
body.leanTrackingState = jsonObject[i]["LeanTrackingState"].asInt();
body.lean.leanX = jsonObject[i]["Lean"]["X"].asDouble();
body.lean.leanY = jsonObject[i]["Lean"]["Y"].asDouble();
body.isTracked = jsonObject[i]["IsTracked"].asBool();
body.trackingId = jsonObject[i]["TrackingId"].asUInt64();
body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt();
body.engaged = jsonObject[i]["Engaged"].asBool();
body.handRightConfidence = jsonObject[i]["HandRightConfidence"].asInt();
body.handRightState = jsonObject[i]["HandRightState"].asInt();
body.handLeftConfidence = jsonObject[i]["HandLeftConfidence"].asInt();
body.handLeftState = jsonObject[i]["HandLeftState"].asInt();
body.appearance.wearingGlasses = jsonObject[i]["Appearance"]["WearingGlasses"].asBool();
body.activities.eyeLeftClosed = jsonObject[i]["Activities"]["EyeLeftClosed"].asBool();
body.activities.eyeRightClosed = jsonObject[i]["Activities"]["EyeRightClosed"].asBool();
body.activities.mouthOpen = jsonObject[i]["Activities"]["MouthOpen"].asBool();
body.activities.mouthMoved = jsonObject[i]["Activities"]["MouthMoved"].asBool();
body.activities.lookingAway = jsonObject[i]["Activities"]["LookingAway"].asBool();
body.expressions.neutral = jsonObject[i]["Expressions"]["Neutral"].asBool();
body.expressions.neutral = jsonObject[i]["Expressions"]["Happy"].asBool();
for(int j=0;j<25;j++)
{
k2_client::JointOrientationAndType JOAT;
k2_client::JointPositionAndState JPAS;
std::string fieldName;
switch (j)
{
case 0: fieldName = "SpineBase";break;
case 1: fieldName = "SpineMid";break;
case 2: fieldName = "Neck";break;
case 3: fieldName = "Head";break;
case 4: fieldName = "ShoulderLeft";break;
case 5: fieldName = "ElbowLeft";break;
case 6: fieldName = "WristLeft";break;
case 7: fieldName = "HandLeft";break;
case 8: fieldName = "ShoulderRight";break;
case 9: fieldName = "ElbowRight";break;
case 10: fieldName = "WristRight";break;
case 11: fieldName = "HandRight";break;
case 12: fieldName = "HipLeft";break;
case 13: fieldName = "KneeLeft";break;
case 14: fieldName = "AnkleLeft";break;
case 15: fieldName = "SpineBase";break;
case 16: fieldName = "HipRight";break;
case 17: fieldName = "KneeRight";break;
case 18: fieldName = "AnkleRight";break;
case 19: fieldName = "FootRight";break;
case 20: fieldName = "SpineShoulder";break;
case 21: fieldName = "HandTipLeft";break;
case 22: fieldName = "ThumbLeft";break;
case 23: fieldName = "HandTipRight";break;
case 24: fieldName = "ThumbRight";break;
}
JOAT.orientation.x = jsonObject[i][fieldName]["Orientation"]["X"].asDouble();
JOAT.orientation.y = jsonObject[i][fieldName]["Orientation"]["Y"].asDouble();
JOAT.orientation.z = jsonObject[i][fieldName]["Orientation"]["Z"].asDouble();
JOAT.orientation.w = jsonObject[i][fieldName]["Orientation"]["W"].asDouble();
JOAT.jointType = jsonObject[i][fieldName]["JointType"].asInt();
JPAS.trackingState = jsonObject[i][fieldName]["TrackingState"].asBool();
JPAS.position.x = jsonObject[i][fieldName]["Position"]["X"].asDouble();
JPAS.position.y = jsonObject[i][fieldName]["Position"]["Y"].asDouble();
JPAS.position.z = jsonObject[i][fieldName]["Position"]["Z"].asDouble();
JPAS.jointType = jsonObject[i][fieldName]["JointType"].asInt();
body.jointOrientations.push_back(JOAT);
body.jointPositions.push_back(JPAS);
}
bodyArray.bodies.push_back(body);
}
}
catch (...)
{
std::cout<<"An exception occured"<<std::endl;
continue;
}
bodyPub.publish(bodyArray);
}
return 0;
}
std::string jsonString(BSONElement &elem, JsonStringFormat format, bool includeFieldNames, int pretty, UUIDEncoding uuidEncoding)
{
BSONType t = elem.type();
int sign;
if ( t == Undefined )
return "undefined";
stringstream s;
if ( includeFieldNames )
s << '"' << escape( elem.fieldName() ) << "\" : ";
switch ( elem.type() ) {
case mongo::String:
case Symbol:
s << '"' << escape( string(elem.valuestr(), elem.valuestrsize()-1) ) << '"';
break;
case NumberLong:
s << "NumberLong(" << elem._numberLong() << ")";
break;
case NumberInt:
case NumberDouble:
if ( elem.number() >= -numeric_limits< double >::max() &&
elem.number() <= numeric_limits< double >::max() ) {
s.precision( 16 );
s << elem.number();
}
else if ( mongo::isNaN(elem.number()) ) {
s << "NaN";
}
else if ( mongo::isInf(elem.number(), &sign) ) {
s << ( sign == 1 ? "Infinity" : "-Infinity");
}
else {
StringBuilder ss;
ss << "Number " << elem.number() << " cannot be represented in JSON";
string message = ss.str();
//massert( 10311 , message.c_str(), false );
}
break;
case mongo::Bool:
s << ( elem.boolean() ? "true" : "false" );
break;
case jstNULL:
s << "null";
break;
case Object: {
BSONObj obj = elem.embeddedObject();
s << jsonString(obj, format, pretty, uuidEncoding);
}
break;
case mongo::Array: {
if ( elem.embeddedObject().isEmpty() ) {
s << "[]";
break;
}
s << "[ ";
BSONObjIterator i( elem.embeddedObject() );
BSONElement e = i.next();
if ( !e.eoo() ) {
int count = 0;
while ( 1 ) {
if( pretty ) {
s << '\n';
for( int x = 0; x < pretty; x++ )
s << " ";
}
if (strtol(e.fieldName(), 0, 10) > count) {
s << "undefined";
}
else {
s << jsonString(e, format, false, pretty?pretty+1:0, uuidEncoding);
e = i.next();
}
count++;
if ( e.eoo() ) {
s << '\n';
for( int x = 0; x < pretty - 1; x++ )
s << " ";
s << "]";
break;
}
s << ", ";
}
}
//s << " ]";
break;
}
case DBRef: {
mongo::OID *x = (mongo::OID *) (elem.valuestr() + elem.valuestrsize());
if ( format == TenGen )
s << "Dbref( ";
else
s << "{ \"$ref\" : ";
s << '"' << elem.valuestr() << "\", ";
if ( format != TenGen )
s << "\"$id\" : ";
s << '"' << *x << "\" ";
if ( format == TenGen )
s << ')';
else
s << '}';
break;
}
case jstOID:
if ( format == TenGen ) {
s << "ObjectId(";
}
else {
s << "{ \"$oid\" : ";
}
s << '"' << elem.__oid() << '"';
if ( format == TenGen ) {
s << ")";
}
else {
s << " }";
}
break;
case BinData: {
int len = *(int *)( elem.value() );
BinDataType type = BinDataType( *(char *)( (int *)( elem.value() ) + 1 ) );
if (type == mongo::bdtUUID || type == mongo::newUUID) {
s << HexUtils::formatUuid(elem, uuidEncoding);
break;
}
s << "{ \"$binary\" : \"";
char *start = ( char * )( elem.value() ) + sizeof( int ) + 1;
base64::encode( s , start , len );
s << "\", \"$type\" : \"" << hex;
s.width( 2 );
s.fill( '0' );
s << type << dec;
s << "\" }";
break;
}
case mongo::Date:
if ( format == Strict )
s << "{ \"$date\" : ";
else
s << "ISODate(";
if( pretty ) {
Date_t d = elem.date();
long long ms = (long long) d.millis;
boost::posix_time::ptime epoch(boost::gregorian::date(1970,1,1));
boost::posix_time::time_duration diff = boost::posix_time::millisec(ms);
boost::posix_time::ptime time = epoch + diff;
std::string timestr = miutil::isotimeString(time, true, true);
s << '"' << timestr << '"';
/* if( d == 0 ) s << '0';
else
//P s << '"' << elem.date().toString() << '"';
s << '"' << elem.date().millis << '"';*/
}
else
s << elem.date();
if ( format == Strict )
s << " }";
else
s << ")";
break;
case RegEx:
if ( format == Strict ) {
s << "{ \"$regex\" : \"" << escape( elem.regex() );
s << "\", \"$options\" : \"" << elem.regexFlags() << "\" }";
}
else {
s << "/" << escape( elem.regex() , true ) << "/";
// FIXME Worry about alpha order?
for ( const char *f = elem.regexFlags(); *f; ++f ) {
switch ( *f ) {
case 'g':
case 'i':
case 'm':
s << *f;
default:
break;
}
}
}
break;
case CodeWScope: {
BSONObj scope = elem.codeWScopeObject();
if ( ! scope.isEmpty() ) {
s << "{ \"$code\" : " << elem._asCode() << " , "
<< " \"$scope\" : " << scope.jsonString() << " }";
break;
}
}
case Code:
s << elem._asCode();
break;
case Timestamp:
if ( format == TenGen ) {
s << "Timestamp(" << ( elem.timestampTime() / 1000 ) << ", " << elem.timestampInc() << ")";
}
else {
s << "{ \"$timestamp\" : { \"t\" : " << ( elem.timestampTime() / 1000 ) << ", \"i\" : " << elem.timestampInc() << " } }";
}
break;
case MinKey:
s << "{ \"$minKey\" : 1 }";
break;
case MaxKey:
s << "{ \"$maxKey\" : 1 }";
break;
default:
StringBuilder ss;
ss << "Cannot create a properly formatted JSON string with "
<< "element: " << elem.toString() << " of type: " << elem.type();
string message = ss.str();
//massert( 10312 , message.c_str(), false );
}
return s.str();
}
int main(int argC,char **argV)
{
ros::init(argC,argV,"startBody");
ros::NodeHandle n;
std::string serverAddress;
n.getParam("/serverNameOrIP",serverAddress);
Socket mySocket(serverAddress.c_str(),const_cast<char*>("9003"), streamSize);
ros::Publisher bodyPub = n.advertise<k2_client::BodyArray>(topicName,1);
char utf16Array[readSkipSize];
char jsonCharArray[stringSize];
char *jsonCharArrayPtr;
char *utf16ArrayPtr;
ros::Rate r(30);
while(ros::ok())
{
mySocket.readData();
jsonCharArrayPtr = jsonCharArray;
utf16ArrayPtr = utf16Array;
memset(utf16Array, 0, sizeof(utf16Array));
memcpy(utf16Array, mySocket.mBuffer, sizeof(utf16Array));
size_t iconv_in = static_cast<size_t>(readSkipSize);
size_t iconv_out = static_cast<size_t>(stringSize);
iconv_t charConverter = iconv_open("UTF-8","UTF-16");
size_t nconv = iconv(charConverter, &utf16ArrayPtr, &iconv_in, &jsonCharArrayPtr, &iconv_out);
iconv_close(charConverter);
if (nconv == (size_t) - 1)
{
if (errno == EINVAL)
ROS_ERROR("An incomplete multibyte sequence has been encountered in the input.");
else if (errno == EILSEQ)
ROS_ERROR("An invalid multibyte sequence has been encountered in the input.");
else
ROS_ERROR("There is not sufficient room at jsonCharArray");
}
double utcTime = 0.0;
memcpy(&utcTime,&mySocket.mBuffer[readSkipSize], sizeof(double));
std::string jsonString(jsonCharArray);
Json::Value jsonObject;
Json::Reader jsonReader;
bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
if(!parsingSuccessful)
{
ROS_ERROR("Failure to parse");
continue;
}
k2_client::BodyArray bodyArray;
try
{
for(int i=0;i<6;i++)
{
k2_client::Body body;
body.header.stamp = ros::Time(utcTime);
body.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
body.leanTrackingState = jsonObject[i]["LeanTrackingState"].asInt();
body.lean.leanX = jsonObject[i]["Lean"]["X"].asDouble();
body.lean.leanY = jsonObject[i]["Lean"]["Y"].asDouble();
body.isTracked = jsonObject[i]["IsTracked"].asBool();
body.trackingId = jsonObject[i]["TrackingId"].asUInt64();
body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt();
body.engaged = jsonObject[i]["Engaged"].asBool();
body.handRightConfidence = jsonObject[i]["HandRightConfidence"].asInt();
body.handRightState = jsonObject[i]["HandRightState"].asInt();
body.handLeftConfidence = jsonObject[i]["HandLeftConfidence"].asInt();
body.handLeftState = jsonObject[i]["HandLeftState"].asInt();
body.appearance.wearingGlasses = jsonObject[i]["Appearance"]["WearingGlasses"].asBool();
body.activities.eyeLeftClosed = jsonObject[i]["Activities"]["EyeLeftClosed"].asBool();
body.activities.eyeRightClosed = jsonObject[i]["Activities"]["EyeRightClosed"].asBool();
body.activities.mouthOpen = jsonObject[i]["Activities"]["MouthOpen"].asBool();
body.activities.mouthMoved = jsonObject[i]["Activities"]["MouthMoved"].asBool();
body.activities.lookingAway = jsonObject[i]["Activities"]["LookingAway"].asBool();
body.expressions.neutral = jsonObject[i]["Expressions"]["Neutral"].asBool();
body.expressions.neutral = jsonObject[i]["Expressions"]["Happy"].asBool();
for(int j=0;j<25;j++)
{
k2_client::JointOrientationAndType JOAT;
k2_client::JointPositionAndState JPAS;
std::string fieldName;
switch (j)
{
case 0: fieldName = "SpineBase";break;
case 1: fieldName = "SpineMid";break;
case 2: fieldName = "Neck";break;
case 3: fieldName = "Head";break;
case 4: fieldName = "ShoulderLeft";break;
case 5: fieldName = "ElbowLeft";break;
case 6: fieldName = "WristLeft";break;
case 7: fieldName = "HandLeft";break;
case 8: fieldName = "ShoulderRight";break;
case 9: fieldName = "ElbowRight";break;
case 10: fieldName = "WristRight";break;
case 11: fieldName = "HandRight";break;
case 12: fieldName = "HipLeft";break;
case 13: fieldName = "KneeLeft";break;
case 14: fieldName = "AnkleLeft";break;
case 15: fieldName = "SpineBase";break;
case 16: fieldName = "HipRight";break;
case 17: fieldName = "KneeRight";break;
case 18: fieldName = "AnkleRight";break;
case 19: fieldName = "FootRight";break;
case 20: fieldName = "SpineShoulder";break;
case 21: fieldName = "HandTipLeft";break;
case 22: fieldName = "ThumbLeft";break;
case 23: fieldName = "HandTipRight";break;
case 24: fieldName = "ThumbRight";break;
}
JOAT.orientation.x = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble();
JOAT.orientation.y = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble();
JOAT.orientation.z = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble();
JOAT.orientation.w = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble();
JOAT.jointType = jsonObject[i]["JointOrientations"][fieldName]["JointType"].asInt();
JPAS.trackingState = jsonObject[i]["Joints"][fieldName]["TrackingState"].asBool();
JPAS.position.x = jsonObject[i]["Joints"][fieldName]["Position"]["X"].asDouble();
JPAS.position.y = jsonObject[i]["Joints"][fieldName]["Position"]["Y"].asDouble();
JPAS.position.z = jsonObject[i]["Joints"][fieldName]["Position"]["Z"].asDouble();
JPAS.jointType = jsonObject[i]["Joints"][fieldName]["JointType"].asInt();
body.jointOrientations.push_back(JOAT);
body.jointPositions.push_back(JPAS);
}
bodyArray.bodies.push_back(body);
}
}
catch (...)
{
ROS_ERROR("An exception occured");
continue;
}
bodyPub.publish(bodyArray);
ros::spinOnce();
r.sleep();
}
return 0;
}
//================= private ==================
void FigureItem::initFromConfigFile(const FigureKey& figureKey)
{
assert(figureKey.isValid());
std::string fileName =
FigureResourceManager::getInstance()->constructPath((figureKey.isMale ? "boy/" : "girl/")
+ figureKey.type + "/" + figureKey.id + "/boneConfig.json");
cocos2d::Data fileData = cocos2d::FileUtils::getInstance()->getDataFromFile(fileName);
if (fileData.getSize() <= 0)
{
cocos2d::log("Fail opening file:%s\n", fileName.c_str());
return;
}
assert(fileData.getBytes() != nullptr);
std::string jsonString((char*)fileData.getBytes(), fileData.getSize());
rapidjson::Document document;
document.Parse<0>(jsonString.c_str());
if (document.HasParseError() || document.IsObject() == false)
{
cocos2d::log("json ParseError %s\n", document.GetParseError());
return;
}
if (document.HasMember("COMPONENTS"))
{
const rapidjson::Value& components = document["COMPONENTS"];
assert(components.IsArray());
for (rapidjson::SizeType i = 0; i < components.Size(); ++i)
{
const rapidjson::Value& item = components[i];
assert(item.HasMember("TYPE"));
assert(item.HasMember("ID"));
std::string type = item["TYPE"].GetString();
std::string id = item["ID"].GetString();
assert(mSonTypeSet.find(type) == mSonTypeSet.end());
mSonTypeSet.insert(type);
mSonFigureKeys.push_back(FigureKey(figureKey.isMale, type, id));
FigureKey sonFigureKey = FigureKey(figureKey.isMale, type, id);
initFromConfigFile(sonFigureKey);
}
return;
}
assert(document.HasMember("BONE_CONFIGS"));
const rapidjson::Value& boneConfigs = document["BONE_CONFIGS"];
assert(boneConfigs.IsArray());
for (rapidjson::SizeType i = 0; i < boneConfigs.Size(); ++i)
{
const rapidjson::Value& boneConfigJson = boneConfigs[i];
std::string boneName = boneConfigJson["BONE_NAME"].GetString();
const rapidjson::Value& bonePicture = boneConfigJson["BONE_PICTURE"];
BoneConfig boneConfig;
boneConfig.boneName = boneName;
if (bonePicture.IsArray())
{
for (rapidjson::SizeType i = 0; i < bonePicture.Size(); ++i)
{
const rapidjson::Value& item = bonePicture[i];
assert(item.HasMember("PATH"));
std::string picturePath =
FigureResourceManager::getInstance()->constructPath(item["PATH"].GetString());
assert(cocos2d::FileUtils::getInstance()->isFileExist(picturePath));
boneConfig.pictures.push_back(picturePath);
}
}
else
{
//bonePicture.GetString() == boy-or-girl/type/id/bone.png
std::string picturePath =
FigureResourceManager::getInstance()->constructPath(bonePicture.GetString());
assert(cocos2d::FileUtils::getInstance()->isFileExist(picturePath));
boneConfig.pictures.push_back(picturePath);
}
mBoneConfigArray.push_back(boneConfig);
}
}
void BoneManager::initDefaultBoneConfigMap(bool isMale)
{
std::map<std::string, std::vector<std::string>>* boneConfigMap = &mBoyDefaultBoneConfigMap;
std::string fileName =
FigureResourceManager::getInstance()->constructPath("boy/defaultBoneConfig.json");
if (isMale == false)
{
fileName = FigureResourceManager::getInstance()->constructPath("girl/defaultBoneConfig.json");
boneConfigMap = &mGirlDefaultBoneConfigMap;
}
cocos2d::Data fileData = cocos2d::FileUtils::getInstance()->getDataFromFile(fileName);
if (fileData.getSize() <= 0)
{
cocos2d::log("Fail opening file:%s\n", fileName.c_str());
return;
}
assert(fileData.getBytes() != nullptr);
std::string jsonString((char*)fileData.getBytes(), fileData.getSize());
rapidjson::Document document;
document.Parse<0>(jsonString.c_str());
if (document.HasParseError() || document.IsObject() == false)
{
cocos2d::log("json ParseError %s\n", document.GetParseError());
return;
}
assert(document.HasMember("COMPONENTS") == false);
assert(document.HasMember("BONE_CONFIGS"));
const rapidjson::Value& boneConfigs = document["BONE_CONFIGS"];
assert(boneConfigs.IsArray());
for (rapidjson::SizeType i = 0; i < boneConfigs.Size(); ++i)
{
const rapidjson::Value& boneConfigJson = boneConfigs[i];
std::string boneName = boneConfigJson["BONE_NAME"].GetString();
const rapidjson::Value& bonePicture = boneConfigJson["BONE_PICTURE"];
std::vector<std::string> pictures;
if (bonePicture.IsArray())
{
for (rapidjson::SizeType i = 0; i < bonePicture.Size(); ++i)
{
const rapidjson::Value& item = bonePicture[i];
assert(item.HasMember("PATH"));
std::string picturePath =
FigureResourceManager::getInstance()->constructPath(item["PATH"].GetString());
assert(cocos2d::FileUtils::getInstance()->isFileExist(picturePath));
pictures.push_back(picturePath);
}
}
else
{
//bonePicture.GetString() == boy-or-girl/type/id/bone.png
std::string picturePath =
FigureResourceManager::getInstance()->constructPath(bonePicture.GetString());
assert(cocos2d::FileUtils::getInstance()->isFileExist(picturePath));
pictures.push_back(picturePath);
}
boneConfigMap->insert(
std::map<std::string, std::vector<std::string>>::value_type(boneName, pictures));
}
}
