void ProtocolAdmin::adminCommandSetOwner(const std::string& param)
{
OutputMessage_ptr output = OutputMessagePool::getInstance()->getOutputMessage(this, false);
if (output) {
boost::char_separator<char> sep(", ");
tokenizer cmdtokens(param, sep);
tokenizer::iterator cmdit = cmdtokens.begin();
std::string _house, name;
_house = parseParams(cmdit, cmdtokens.end());
name = parseParams(cmdit, cmdtokens.end());
trimString(_house);
trimString(name);
if (House* house = Houses::getInstance().getHouse(atoi(_house.c_str()))) {
uint32_t _guid;
if (IOLoginData::getInstance()->getGuidByName(_guid, name)) {
house->setHouseOwner(_guid);
addLogLine(this, LOGTYPE_EVENT, 1, "set " + name + " as new owner of house with id " + _house);
output->AddByte(AP_MSG_COMMAND_OK);
} else {
addLogLine(this, LOGTYPE_WARNING, 1, "Could not find player with name: " + name);
output->AddByte(AP_MSG_COMMAND_FAILED);
output->AddString("such player does not exists");
}
} else {
addLogLine(this, LOGTYPE_WARNING, 1, "Could not find house with id: " + _house);
output->AddByte(AP_MSG_COMMAND_FAILED);
output->AddString("such house does not exists");
}
OutputMessagePool::getInstance()->send(output);
}
}
void Commands::addSkill(Player* player, const std::string& cmd, const std::string& param)
{
boost::char_separator<char> sep(",");
tokenizer cmdtokens(param, sep);
tokenizer::iterator cmdit = cmdtokens.begin();
std::string param1, param2;
param1 = parseParams(cmdit, cmdtokens.end());
param2 = parseParams(cmdit, cmdtokens.end());
trimString(param1);
trimString(param2);
toLowerCaseString(param2);
Player* paramPlayer = g_game.getPlayerByName(param1);
if (!paramPlayer) {
player->sendTextMessage(MSG_STATUS_SMALL, "Couldn't find target.");
return;
}
if (param2[0] == 'l' || param2[0] == 'e') {
paramPlayer->addExperience(Player::getExpForLevel(paramPlayer->getLevel() + 1) - paramPlayer->experience, false, false);
} else if (param2[0] == 'm') {
paramPlayer->addManaSpent(paramPlayer->vocation->getReqMana(paramPlayer->getBaseMagicLevel() + 1) - paramPlayer->manaSpent, false);
} else {
skills_t skillId = getSkillId(param2);
paramPlayer->addSkillAdvance(skillId, paramPlayer->vocation->getReqSkillTries(skillId, paramPlayer->getSkill(skillId, SKILL_LEVEL) + 1));
}
}
//---------------------------------------------------------------------------------------------------
SFBool CFunction::setValueByName(const SFString& fieldName, const SFString& fieldValue)
{
switch (tolower(fieldName[0]))
{
case 'a':
if ( fieldName % "anonymous" ) { anonymous = toBool(fieldValue); return TRUE; }
break;
case 'c':
if ( fieldName % "constant" ) { constant = toBool(fieldValue); return TRUE; }
break;
case 'e':
if ( fieldName % "encoding" ) { encoding = fieldValue; return TRUE; }
break;
case 'h':
if ( fieldName % "handle" ) { handle = toLong(fieldValue); return TRUE; }
break;
case 'i':
if ( fieldName % "indexed" ) { indexed = toBool(fieldValue); return TRUE; }
if ( fieldName % "inputs" ) parseParams(TRUE, fieldValue); return TRUE;
break;
case 'n':
if ( fieldName % "name" ) { name = fieldValue; return TRUE; }
break;
case 'o':
if ( fieldName % "outputs" ) parseParams(FALSE, fieldValue); return TRUE;
break;
case 't':
if ( fieldName % "type" ) { type = fieldValue; return TRUE; }
break;
default:
break;
}
return FALSE;
}
Boonas::Boonas(int argc, char** argv)
{
cursor = 0;
orig = new LNHHolder();
temp = 0;
colorValue = 0;
Transformation matrix;
if(argc < 3)
{
return;
}
if(strcmp(argv[1], "polygon") == 0) // DONE
{
polygon(argc, argv); // default the color to a grey, or a blue
}
else if(strcmp(argv[1], "polygonC") == 0)
{
polygonC(argc, argv);
}
else if(strcmp(argv[1], "assemble") == 0) // DONE
{
assemble(argc, argv);
}
else if(strcmp(argv[1], "transform") == 0) //WORKS
{
loadFile(argv[2]); // loads the data into a LNHHolder
matrix = parseParams(3, argc, argv, matrix);
doMult(matrix); // applies the matrix transformation
writeFile(argv[2]); // writes file;
}
else if(strcmp(argv[1], "create") == 0) // WORKS
{
loadFile(argv[3]);
matrix = parseParams(4, argc, argv, matrix);
doMult(matrix);
writeFile(argv[2]);
}
else if(strcmp(argv[1], "extrude") == 0) // WORKS
{
loadFile(argv[2]);
doExtrude(argv[4], argv[3]); // extrudes argv 3 distance
}
else if(strcmp(argv[1], "clip") == 0) // clip, file, inpolies, outpolies, normalx normaly normalz, pointx, pointy, pointz
{
loadFile(argv[2]);
//doClip(argv[3], argv[4], argv[5], argv[6], argv[7], argv[8], argv[9], argv[10]);
doClip(argv[3], argv[4], argv[5], argv[6], argv[7], argv[8], argv[9]);
}
else if(strcmp(argv[1], "rot") == 0) // rot infile outfile vectorx, y, z pointx, y, z, divisions, degrees
{
loadFile(argv[2]);
doRotate(argv[3], argv[4], argv[5], argv[6], argv[7], argv[8], argv[9], argv[10], argv[11]);
writeFile(argv[3]);
}
return;
}
CRESTRequestRef CRESTRequestCreateWithBytes(UInt8 const *bytes, ssize_t len)
{
CFURLRef url;
CRESTRequestRef request;
CFHTTPMessageRef message;
message = CFHTTPMessageCreateEmpty(kCFAllocatorDefault, true);
request = calloc(1, sizeof(struct CRESTRequest));
assert(request);
bool isAppended = CFHTTPMessageAppendBytes(message, bytes, (CFIndex) len);
bool isComplete = CFHTTPMessageIsHeaderComplete(message);
if( ! ( isAppended && isComplete ) )
{
CFRelease(message);
CRESTRequestDealloc(request);
return NULL;
}
url = CFHTTPMessageCopyRequestURL(message);
CFMutableDictionaryRef params = CFDictionaryCreateMutable(kCFAllocatorDefault, 0, &kCFTypeDictionaryKeyCallBacks,
&kCFTypeDictionaryValueCallBacks);
CFStringRef query = CFURLCopyQueryString(url, NULL);
if( query )
{
parseParams(params, query);
CFRelease(query);
}
CFDataRef bodyData = CFHTTPMessageCopyBody(message);
if( CFDataGetLength(bodyData) )
{
CFStringRef bodyString = CFStringCreateFromExternalRepresentation(kCFAllocatorDefault, bodyData,
kCFStringEncodingUTF8);
parseParams(params, bodyString);
CFRelease(bodyString);
}
CFRelease(bodyData);
request->method = CFHTTPMessageCopyRequestMethod(message);
request->path = CFURLCopyPath(url);
request->params = (CFDictionaryRef) params;
CFRelease(url);
CFRelease(message);
return request;
}
int main(int argc, char* argv[]) {
drs_t drs;
config_t config;
size_t dirEntries;
char *contents = argv[0]; // temporary
int rc = 0;
if (parseParams(argc, argv, &config)) {
usage();
return 1;
}
if (config.extract) {
rc = drs_load(config.filePath, &drs);
if (rc) {
printf("RETURNED %d\n", rc);
} else {
drs_print_header(&drs, stdout);
drs_extract_archive(&drs, "drsFiles");
//drs_create_archive(&drs, "../generated.drs");
drs_free(&drs);
}
} else {
drs_init_empty(&drs);
rc = directory_scan(config.filePath, contents, &dirEntries);
}
return rc;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "joint_to_cart");
ros::NodeHandle nh;
ros::NodeHandle _nh("~");
mRobot = new RTKRobotArm(true);
if(!mRobot->initialize(_nh)) {
ROS_ERROR("Error while loading robot");
return 1;
}
if(!parseParams(_nh)) {
ROS_ERROR("Errors while parsing arguments.");
return 1;
}
numdof = mRobot->numdof;
read_torque.resize(numdof);
read_jpos.resize(numdof);
ee_ft.resize(6);
pub_pose = nh.advertise<geometry_msgs::PoseStamped>(output_cart_pose, 3);
pub_ft = nh.advertise<geometry_msgs::WrenchStamped>(output_cart_ft, 3);
ros::Subscriber sub = nh.subscribe<sensor_msgs::JointState>(input_joint_topic, 10, jointStateCallback,ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_ft = nh.subscribe<geometry_msgs::WrenchStamped>("/right_arm_ft_sensor/wrench", 10, sensorFTCallback, ros::TransportHints().tcpNoDelay());
ROS_INFO("Node started");
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
Object *object;
tObject tobject;
CVWindow main;
Params params;
if (!parseParams(¶ms, argc, argv))
{
printUsage();
return -1;
}
object = new Object(&Shared::database);
if (!object->findObjects(params.frameID))
{
printf("frame not found\n");
return -1;
}
main.createWindow("main", 0);
while (object->nextObject(tobject))
{
main.showImage(tobject.object_image);
CVWindow::waitKey(-1);
}
return 0;
}
Param_t * stringbreaker(char*str, Param_t *param)
{
char *t; //create new temporary character array
char *stringcount[MAXARGS]; //create array of character pointers
int i=0;
t=strtok(str," \n\t"); //tokenize first argument based on where " ", "\t", and "\n" are located
printf("Temp String: %s\n",t); //print the temporary string
//tokenize the rest of the string
while(t != NULL)
{
printf("\nThe token is: %s", t);
stringcount[i] = t; //add temp char to string array
param->argumentCount++; //increment argc
t = strtok(NULL," \n\t"); //tokenize
i++;
}
parseParams(stringcount,param); //
printParams(param); //print params, returns nothing
//theshell(param) //fork to new process id, show child process and wait
param = clearParam(param);
return param;
}
std::string MyParser::parsePath(const std::string& path, std::vector<Query>* params) const {
int i1 = path.find_first_of("?");
if (i1 == std::string::npos) { // no query params
return path;
}
parseParams(path.substr(i1 + 1), params);
return path.substr(0, i1);
}
list<Param*> Parser::parseParamRest()
{
if ( check( COMMA_T ) )
{
match( COMMA_T );
list<Param*> params = parseParams();
return params;
}
else
return list<Param*>();
}
int main (int argc, char *argv[])
{
//init input
input.lambda = 0.5;
input.mu = 0.35;
input.r = 1.5;
input.B = 10;
input.P = 3;
input.numToArrive = 20;
input.fileName[0] = '\0';
parseParams(argc, argv, &input);
if(FALSE == checkInput(&input))
exit(-1);
//output parameters (alread loaded file)
printf("Emulation Parameters:\n");
if(!fptr)
{
printf("lambda = %.1f\n", input.lambda);
printf("mu = %.2f\n", input.mu);
}
printf("r = %.1f\n", input.r);
printf("B = %d\n", input.B);
if(!fptr)
{
printf("P = %d\n", input.P);
printf("number to arrive = %d\n", input.numToArrive);
}
else
printf("tsfile = %s\n", input.fileName);
//////////////////////////////////////////////
// init queues
MyQueueInit(&arrivalQueue);
MyQueueInit(&serverQueue);
g_tokenSize = input.B;
gettimeofday(&emulationStartTime, &tz);
printMiniSeconds(emulationStartTime, emulationStartTime);
printf("emulation begins\n");
pthread_create(&arrive_thread, 0, arriveThread, NULL);
pthread_create(&server_thread, 0, serverThread, NULL);
pthread_create(&token_thread, 0, tokenThread, NULL);
//when to close the file??
}
uint16_t CommandTerminal::readSerial() {
while (Serial.available()) {
delay(1);
char c = Serial.read();
if (c == '\n') {
parseParams();
return numParams;
}
g_serialString += c;
return 0;
}
}
list<Param*> Parser::parseParamList()
{
if ( check( LEFTPARENTH_T ) )
{
match( LEFTPARENTH_T );
list<Param*> params = parseParams();
match( RIGHTPARENTH_T );
return params;
}
else
return list<Param*>();
}
void MyParser::parsePayload(const std::string& payload, std::vector<Query>* out) const {
int i1 = payload.find_first_of("&");
if (i1 == std::string::npos) {
int i2 = payload.find_first_of("=");
if (i2 != std::string::npos) {
auto query = parseQuery(payload);
out->push_back(query);
return; // single item
}
return; // no payload items
}
parseParams(payload, out);
}
bool
ContentChecker::contentIsValid(const StringDict &content) const
{
try {
if (!content["&content-type"].compare("text/xml"))
m_parsedParams = parseParams(content);
else
throw std::runtime_error("Unknown content type: " + content["&content-type"]);
}
catch (const std::exception &err) {
m_err = err.what();
return false;
}
return true;
}
ESP_SBC_Command::ESP_SBC_Command(std::string input) {
//ctor
parseCorrect = false;
doc.Parse((const char*)input.c_str(), 0, TIXML_ENCODING_UTF8);
TiXmlElement* element = doc.FirstChildElement(XML_PAID_COMMAND);
if (element) {
name = ESP_XmlUtils::queryStringValue(element,"name");
dest = ESP_XmlUtils::queryStringValue(element,"dest");
cmd_id = ESP_XmlUtils::queryStringValue(element,"id");
if (cmd_id != "") {
parseCorrect = true;
outputUL20 = "cmdid|"+cmd_id+"#";
}
parseParams(element);
}
}
/**
* Create [gnuplot](http://www.gnuplot.info/) ".gpl" file from ".dat" binary file with header section.
* This program is developed to plot the input file of [maxximino/dpacalc](https://github.com/maxximino/dpacalc)
*/
int main(int argc, char** argv) {
parseParams(argc, argv);
openFiles();
parseInputFile();
createDat();
createGpl();
closeFiles();
cout << endl << "[FINISHED]" << endl << endl;
return 0;
}
void generateServerStatus( string url , string& responseMsg ){
static vector<string> commands;
if ( commands.size() == 0 ){
commands.push_back( "serverStatus" );
commands.push_back( "buildinfo" );
}
BSONObj params;
if ( url.find( "?" ) != string::npos ) {
parseParams( params , url.substr( url.find( "?" ) + 1 ) );
}
BSONObjBuilder buf(1024);
for ( unsigned i=0; i<commands.size(); i++ ){
string cmd = commands[i];
Command * c = Command::findCommand( cmd );
assert( c );
assert( c->locktype() == 0 );
BSONObj co;
{
BSONObjBuilder b;
b.append( cmd.c_str() , 1 );
if ( cmd == "serverStatus" && params["repl"].type() ){
b.append( "repl" , atoi( params["repl"].valuestr() ) );
}
co = b.obj();
}
string errmsg;
BSONObjBuilder sub;
if ( ! c->run( "admin.$cmd" , co , errmsg , sub , false ) )
buf.append( cmd.c_str() , errmsg );
else
buf.append( cmd.c_str() , sub.obj() );
}
responseMsg = buf.obj().jsonString();
}
int main(int argc, char *argv[]){
if(argc==2) {
int correctSyntax=checkSyntax(argv[1]);
if(correctSyntax){
printf("Sintaxe correcta\n");
char *user,*password,*host,*urlpath,*ip;
int ret,sock1fd,pasvPort,sock2fd;
user=ALLOCSTRING;
password=ALLOCSTRING;
host=ALLOCSTRING;
urlpath=ALLOCSTRING;
ip=ALLOCSTRING;
ret = parseParams(argv[1],user,password,host,urlpath);
getIP(host,ip);
sock1fd=createSocket(ip,FTPPORT);
if(ret == 0)
loginUserPass(sock1fd,user,password);
else if(ret == 1)
loginUser(sock1fd,user);
else if(ret == 2)
loginUserPass(sock1fd,"anonymous","foo");
pasvPort = enterPassiveMode(sock1fd);
sock2fd = createSocket(ip,pasvPort);
download(sock1fd,sock2fd,urlpath);
close(sock1fd);
close(sock2fd);
}
else printf("Sintaxe errada!\n");
}
else return -1;
return 0;
}
// hlavni funkce main
int main(int argc, char* argv[])
{
ProgParams state;
Error err = parseParams(argc, argv, &state);
CHECK_ERROR(err);
switch (state.progState) {
case PS_HELP:
printHelp();
break;
case PS_WEIGHTED_ARITHMETIC_MEAN:
err = computeWeightedMean(stdin, stdout, WM_ARITHMETIC);
CHECK_ERROR(err);
break;
case PS_WEIGHTED_QUADRATIC_MEAN:
err = computeWeightedMean(stdin, stdout, WM_QUADRATIC);
CHECK_ERROR(err);
break;
case PS_HYPERBOLIC_TANGENT:
err = computeTanh(stdin, stdout, sigdigToEpsylon(state.sigdig));
CHECK_ERROR(err);
break;
case PS_LOGARITHM:
err = computeLog(stdin, stdout, state.logBase, sigdigToEpsylon(state.sigdig));
CHECK_ERROR(err);
break;
default:
printError(E_NOT_IMPLEMENTED);
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
parsing_result_type_t parseSimulation(xmlNode * node, CONTEXT * ctx)
{
ctx->simspec = calloc(1, sizeof(st_simspec));
char *type_str;
if ( getStr(node, "signal_type", &type_str)
&& getInt(node, "signal_no", &ctx->simspec->signal_no, PARSER_REQUIRED)
&& getInt(node, "bit_length", &ctx->simspec->bit_length, PARSER_REQUIRED) )
{
if ( find_duplicate(ctx, ctx->simspec->signal_no, ctx->simspec->bit_length) )
{
printf("Duplicate signal number %d (bit_length %d) for device %s (position %d)\n",
ctx->simspec->signal_no, ctx->simspec->bit_length,
ctx->device->name, ctx->device->position);
assert(0);
}
ctx->simspec->type = parseStType(type_str);
return parseParams(node, ctx->simspec)
&& ( ctx->simspec->parent = ctx->device )
&& ellAddOK(&ctx->device->simspecs, &ctx->simspec->node);
}
else
return PARSING_ERROR;
}
// ---------------------------------------------------------------- //
bool CommandInterpreter::parse( CommandProcessor& cp, const char* command )
{
if (!command)
return false;
if(command[0] == '#') //Comment
return true;
// Split the command into full command name and parameter list
DynArray<string> split;
{ FOREACH_TOKEN( token, command, "()" ) split.add( token ); }
if( split.size() < 1 || split.size() > 2 ) return false;
const char* strFullCommandName = "EMPTY";
const char* strParams = "EMPTY";
if(split.size() >= 1) {
strFullCommandName = split[0].c_str();
if(split.size() == 2)
strParams = split[1].c_str();
}
// Request the command
CommandRef commandRef = cp.requestCommand( strFullCommandName );
if( commandRef && commandRef->object )
{
CommandParameters* cmdParams;
parseParams( cp, commandRef, strParams, cmdParams );
// Call the command
commandRef->action->call( commandRef->object, *cmdParams );
delete cmdParams;
return true;
}
parseErrors.add("Command not found.");
return false;
}
bool RegressTest::init(int argc, char **argv)
{
//TBD: Perhaps allow option for specifying report file.
_fpReportFile = stdout;
if (!parseParams(argc, argv)) {
return false;
}
//make banner title for report
fprintf(_fpReportFile, "\n\n***********************************************************\n\n");
fprintf(_fpReportFile, "REGRESSION TEST FOR BEDTOOLS SUB-PROGRAM: %s\n", _subProgram.c_str());
fprintf(_fpReportFile, "\n***********************************************************\n\n");
//setup output directory for testing's temporary files and generated data files.
_tmpDirname = "tempTesting_";
time_t currTime = time(NULL);
string timeStr = ctime(&currTime); //timeStr will equal Www Mmm dd hh:mm:ss yyyy followed by newline and null char.
//chomp the newLine
timeStr.erase(timeStr.size()-1);
//adjust timeStr to change spaces to underscores.
for (int i=0; i < (int)timeStr.size(); i++) {
if (isspace(timeStr[i])) {
timeStr[i] = '_';
} else if (timeStr[i] == ':') {
timeStr[i] = '-';
}
}
_tmpDirname += timeStr;
int mkdirRetval = mkdir(_tmpDirname.c_str(), S_IRWXU | S_IRWXG | S_IRWXO ); //mkdir directory with all permissions allowed.
if (mkdirRetval != 0) {
fprintf(stderr, "Error: Unable to create temporary output directory %s.\n", _tmpDirname.c_str());
return false;
}
_tmpDirname += "/";
_memoryLogfilename = _tmpDirname + "memoryLog.txt";
_userName = getenv("USER");
if ((int)_filePrecessorOptions.size() != _filesPerRun) {
fprintf(stderr, "Error: Sub-program specific filesPerRun number must match number of precessor options.\n");
return false;
}
if (!config()) {
return false;
}
if ((int)_correctFiles->size() % _filesPerRun != 0) {
fprintf(stderr, "Error: number of correctness files is not evenly divisible by number of files per run.\n");
return false;
}
if ((int)_performFiles->size() % _filesPerRun != 0) {
fprintf(stderr, "Error: number of performance files is not evenly divisible by number of files per run.\n");
return false;
}
return true;
}
static void
httpProcessInput(rfbScreenInfoPtr rfbScreen)
{
#ifdef LIBVNCSERVER_IPv6
struct sockaddr_storage addr;
char host[1024];
#else
struct sockaddr_in addr;
#endif
socklen_t addrlen = sizeof(addr);
char fullFname[512];
char params[1024];
char *ptr;
char *fname;
unsigned int maxFnameLen;
FILE* fd;
rfbBool performSubstitutions = FALSE;
char str[256+32];
#ifndef WIN32
char* user=getenv("USER");
#endif
cl.sock=rfbScreen->httpSock;
if (strlen(rfbScreen->httpDir) > 255) {
rfbErr("-httpd directory too long\n");
httpCloseSock(rfbScreen);
return;
}
strcpy(fullFname, rfbScreen->httpDir);
fname = &fullFname[strlen(fullFname)];
maxFnameLen = 511 - strlen(fullFname);
buf_filled=0;
/* Read data from the HTTP client until we get a complete request. */
while (1) {
ssize_t got;
if (buf_filled > sizeof (buf)) {
rfbErr("httpProcessInput: HTTP request is too long\n");
httpCloseSock(rfbScreen);
return;
}
got = read (rfbScreen->httpSock, buf + buf_filled,
sizeof (buf) - buf_filled - 1);
if (got <= 0) {
if (got == 0) {
rfbErr("httpd: premature connection close\n");
} else {
#ifdef WIN32
errno=WSAGetLastError();
#endif
if (errno == EAGAIN) {
return;
}
rfbLogPerror("httpProcessInput: read");
}
httpCloseSock(rfbScreen);
return;
}
buf_filled += got;
buf[buf_filled] = '\0';
/* Is it complete yet (is there a blank line)? */
if (strstr (buf, "\r\r") || strstr (buf, "\n\n") ||
strstr (buf, "\r\n\r\n") || strstr (buf, "\n\r\n\r"))
break;
}
/* Process the request. */
if(rfbScreen->httpEnableProxyConnect) {
const static char* PROXY_OK_STR = "HTTP/1.0 200 OK\r\nContent-Type: octet-stream\r\nPragma: no-cache\r\n\r\n";
if(!strncmp(buf, "CONNECT ", 8)) {
if(atoi(strchr(buf, ':')+1)!=rfbScreen->port) {
rfbErr("httpd: CONNECT format invalid.\n");
rfbWriteExact(&cl,INVALID_REQUEST_STR, strlen(INVALID_REQUEST_STR));
httpCloseSock(rfbScreen);
return;
}
/* proxy connection */
rfbLog("httpd: client asked for CONNECT\n");
rfbWriteExact(&cl,PROXY_OK_STR,strlen(PROXY_OK_STR));
rfbNewClientConnection(rfbScreen,rfbScreen->httpSock);
rfbScreen->httpSock = -1;
return;
}
if (!strncmp(buf, "GET ",4) && !strncmp(strchr(buf,'/'),"/proxied.connection HTTP/1.", 27)) {
/* proxy connection */
rfbLog("httpd: client asked for /proxied.connection\n");
rfbWriteExact(&cl,PROXY_OK_STR,strlen(PROXY_OK_STR));
rfbNewClientConnection(rfbScreen,rfbScreen->httpSock);
rfbScreen->httpSock = -1;
return;
}
}
if (strncmp(buf, "GET ", 4)) {
rfbErr("httpd: no GET line\n");
httpCloseSock(rfbScreen);
return;
} else {
/* Only use the first line. */
buf[strcspn(buf, "\n\r")] = '\0';
}
if (strlen(buf) > maxFnameLen) {
rfbErr("httpd: GET line too long\n");
httpCloseSock(rfbScreen);
return;
}
if (sscanf(buf, "GET %s HTTP/1.", fname) != 1) {
rfbErr("httpd: couldn't parse GET line\n");
httpCloseSock(rfbScreen);
return;
}
if (fname[0] != '/') {
rfbErr("httpd: filename didn't begin with '/'\n");
rfbWriteExact(&cl, NOT_FOUND_STR, strlen(NOT_FOUND_STR));
httpCloseSock(rfbScreen);
return;
}
getpeername(rfbScreen->httpSock, (struct sockaddr *)&addr, &addrlen);
#ifdef LIBVNCSERVER_IPv6
if(getnameinfo((struct sockaddr*)&addr, addrlen, host, sizeof(host), NULL, 0, NI_NUMERICHOST) != 0) {
rfbLogPerror("httpProcessInput: error in getnameinfo");
}
rfbLog("httpd: get '%s' for %s\n", fname+1, host);
#else
rfbLog("httpd: get '%s' for %s\n", fname+1,
inet_ntoa(addr.sin_addr));
#endif
/* Extract parameters from the URL string if necessary */
params[0] = '\0';
ptr = strchr(fname, '?');
if (ptr != NULL) {
*ptr = '\0';
if (!parseParams(&ptr[1], params, 1024)) {
params[0] = '\0';
rfbErr("httpd: bad parameters in the URL\n");
}
}
/* If we were asked for '/', actually read the file index.vnc */
if (strcmp(fname, "/") == 0) {
strcpy(fname, "/index.vnc");
rfbLog("httpd: defaulting to '%s'\n", fname+1);
}
/* Substitutions are performed on files ending .vnc */
if (strlen(fname) >= 4 && strcmp(&fname[strlen(fname)-4], ".vnc") == 0) {
performSubstitutions = TRUE;
}
/* Open the file */
if ((fd = fopen(fullFname, "r")) == 0) {
rfbLogPerror("httpProcessInput: open");
rfbWriteExact(&cl, NOT_FOUND_STR, strlen(NOT_FOUND_STR));
httpCloseSock(rfbScreen);
return;
}
if(performSubstitutions) /* is the 'index.vnc' file */
rfbWriteExact(&cl, OK_STR_HTML, strlen(OK_STR_HTML));
else
rfbWriteExact(&cl, OK_STR, strlen(OK_STR));
while (1) {
int n = fread(buf, 1, BUF_SIZE-1, fd);
if (n < 0) {
rfbLogPerror("httpProcessInput: read");
fclose(fd);
httpCloseSock(rfbScreen);
return;
}
if (n == 0)
break;
if (performSubstitutions) {
/* Substitute $WIDTH, $HEIGHT, etc with the appropriate values.
This won't quite work properly if the .vnc file is longer than
BUF_SIZE, but it's reasonable to assume that .vnc files will
always be short. */
char *ptr = buf;
char *dollar;
buf[n] = 0; /* make sure it's null-terminated */
while ((dollar = strchr(ptr, '$'))!=NULL) {
rfbWriteExact(&cl, ptr, (dollar - ptr));
ptr = dollar;
if (compareAndSkip(&ptr, "$WIDTH")) {
sprintf(str, "%d", rfbScreen->width);
rfbWriteExact(&cl, str, strlen(str));
} else if (compareAndSkip(&ptr, "$HEIGHT")) {
sprintf(str, "%d", rfbScreen->height);
rfbWriteExact(&cl, str, strlen(str));
} else if (compareAndSkip(&ptr, "$APPLETWIDTH")) {
sprintf(str, "%d", rfbScreen->width);
rfbWriteExact(&cl, str, strlen(str));
} else if (compareAndSkip(&ptr, "$APPLETHEIGHT")) {
sprintf(str, "%d", rfbScreen->height + 32);
rfbWriteExact(&cl, str, strlen(str));
} else if (compareAndSkip(&ptr, "$PORT")) {
sprintf(str, "%d", rfbScreen->port);
rfbWriteExact(&cl, str, strlen(str));
} else if (compareAndSkip(&ptr, "$DESKTOP")) {
rfbWriteExact(&cl, rfbScreen->desktopName, strlen(rfbScreen->desktopName));
} else if (compareAndSkip(&ptr, "$DISPLAY")) {
sprintf(str, "%s:%d", rfbScreen->thisHost, rfbScreen->port-5900);
rfbWriteExact(&cl, str, strlen(str));
} else if (compareAndSkip(&ptr, "$USER")) {
#ifndef WIN32
if (user) {
rfbWriteExact(&cl, user,
strlen(user));
} else
#endif
rfbWriteExact(&cl, "?", 1);
} else if (compareAndSkip(&ptr, "$PARAMS")) {
if (params[0] != '\0')
rfbWriteExact(&cl, params, strlen(params));
} else {
if (!compareAndSkip(&ptr, "$$"))
ptr++;
if (rfbWriteExact(&cl, "$", 1) < 0) {
fclose(fd);
httpCloseSock(rfbScreen);
return;
}
}
}
if (rfbWriteExact(&cl, ptr, (&buf[n] - ptr)) < 0)
break;
} else {
/* For files not ending .vnc, just write out the buffer */
if (rfbWriteExact(&cl, buf, n) < 0)
break;
}
}
fclose(fd);
httpCloseSock(rfbScreen);
}
static void
httpProcessInput()
{
struct sockaddr_in addr;
int addrlen = sizeof(addr);
char fullFname[512];
char params[1024];
char *ptr;
char *fname;
int maxFnameLen;
int fd;
Bool performSubstitutions = FALSE;
char str[256];
struct passwd *user;
user = getpwuid(getuid());
if (strlen(httpDir) > 255) {
rfbLog("-httpd directory too long\n");
httpCloseSock();
return;
}
strcpy(fullFname, httpDir);
fname = &fullFname[strlen(fullFname)];
maxFnameLen = 511 - strlen(fullFname);
/* Read data from the HTTP client until we get a complete request. */
while (1) {
ssize_t got = read (httpSock, buf + buf_filled,
sizeof (buf) - buf_filled - 1);
if (got <= 0) {
if (got == 0) {
rfbLog("httpd: premature connection close\n");
} else {
if (errno == EAGAIN) {
return;
}
rfbLogPerror("httpProcessInput: read");
}
httpCloseSock();
return;
}
buf_filled += got;
buf[buf_filled] = '\0';
/* Is it complete yet (is there a blank line)? */
if (strstr (buf, "\r\r") || strstr (buf, "\n\n") ||
strstr (buf, "\r\n\r\n") || strstr (buf, "\n\r\n\r"))
break;
}
/* Process the request. */
if (strncmp(buf, "GET ", 4)) {
rfbLog("httpd: no GET line\n");
httpCloseSock();
return;
} else {
/* Only use the first line. */
buf[strcspn(buf, "\n\r")] = '\0';
}
if (strlen(buf) > maxFnameLen) {
rfbLog("httpd: GET line too long\n");
httpCloseSock();
return;
}
if (sscanf(buf, "GET %s HTTP/1.0", fname) != 1) {
rfbLog("httpd: couldn't parse GET line\n");
httpCloseSock();
return;
}
if (fname[0] != '/') {
rfbLog("httpd: filename didn't begin with '/'\n");
WriteExact(httpSock, NOT_FOUND_STR, strlen(NOT_FOUND_STR));
httpCloseSock();
return;
}
if (strchr(fname+1, '/') != NULL) {
rfbLog("httpd: asking for file in other directory\n");
WriteExact(httpSock, NOT_FOUND_STR, strlen(NOT_FOUND_STR));
httpCloseSock();
return;
}
getpeername(httpSock, (struct sockaddr *)&addr, &addrlen);
rfbLog("httpd: get '%s' for %s\n", fname+1,
inet_ntoa(addr.sin_addr));
/* Extract parameters from the URL string if necessary */
params[0] = '\0';
ptr = strchr(fname, '?');
if (ptr != NULL) {
*ptr = '\0';
if (!parseParams(&ptr[1], params, 1024)) {
params[0] = '\0';
rfbLog("httpd: bad parameters in the URL\n");
}
}
/* If we were asked for '/', actually read the file index.vnc */
if (strcmp(fname, "/") == 0) {
strcpy(fname, "/index.vnc");
rfbLog("httpd: defaulting to '%s'\n", fname+1);
}
/* Substitutions are performed on files ending .vnc */
if (strlen(fname) >= 4 && strcmp(&fname[strlen(fname)-4], ".vnc") == 0) {
performSubstitutions = TRUE;
}
/* Open the file */
if ((fd = open(fullFname, O_RDONLY)) < 0) {
rfbLogPerror("httpProcessInput: open");
WriteExact(httpSock, NOT_FOUND_STR, strlen(NOT_FOUND_STR));
httpCloseSock();
return;
}
WriteExact(httpSock, OK_STR, strlen(OK_STR));
while (1) {
int n = read(fd, buf, BUF_SIZE-1);
if (n < 0) {
rfbLogPerror("httpProcessInput: read");
close(fd);
httpCloseSock();
return;
}
if (n == 0)
break;
if (performSubstitutions) {
/* Substitute $WIDTH, $HEIGHT, etc with the appropriate values.
This won't quite work properly if the .vnc file is longer than
BUF_SIZE, but it's reasonable to assume that .vnc files will
always be short. */
char *ptr = buf;
char *dollar;
buf[n] = 0; /* make sure it's null-terminated */
while (dollar = strchr(ptr, '$')) {
WriteExact(httpSock, ptr, (dollar - ptr));
ptr = dollar;
if (compareAndSkip(&ptr, "$WIDTH")) {
sprintf(str, "%d", rfbScreen.width);
WriteExact(httpSock, str, strlen(str));
} else if (compareAndSkip(&ptr, "$HEIGHT")) {
sprintf(str, "%d", rfbScreen.height);
WriteExact(httpSock, str, strlen(str));
} else if (compareAndSkip(&ptr, "$APPLETWIDTH")) {
sprintf(str, "%d", rfbScreen.width);
WriteExact(httpSock, str, strlen(str));
} else if (compareAndSkip(&ptr, "$APPLETHEIGHT")) {
sprintf(str, "%d", rfbScreen.height + 32);
WriteExact(httpSock, str, strlen(str));
} else if (compareAndSkip(&ptr, "$PORT")) {
sprintf(str, "%d", rfbPort);
WriteExact(httpSock, str, strlen(str));
} else if (compareAndSkip(&ptr, "$DESKTOP")) {
WriteExact(httpSock, desktopName, strlen(desktopName));
} else if (compareAndSkip(&ptr, "$DISPLAY")) {
sprintf(str, "%s:%s", rfbThisHost, display);
WriteExact(httpSock, str, strlen(str));
} else if (compareAndSkip(&ptr, "$USER")) {
if (user) {
WriteExact(httpSock, user->pw_name,
strlen(user->pw_name));
} else {
WriteExact(httpSock, "?", 1);
}
} else if (compareAndSkip(&ptr, "$PARAMS")) {
if (params[0] != '\0')
WriteExact(httpSock, params, strlen(params));
} else {
if (!compareAndSkip(&ptr, "$$"))
ptr++;
if (WriteExact(httpSock, "$", 1) < 0) {
close(fd);
httpCloseSock();
return;
}
}
}
if (WriteExact(httpSock, ptr, (&buf[n] - ptr)) < 0)
break;
} else {
/* For files not ending .vnc, just write out the buffer */
if (WriteExact(httpSock, buf, n) < 0)
break;
}
}
close(fd);
httpCloseSock();
}
//1- porta
//2- modo (SEND | RECEIVE)
//3- filepath
int main(int argc, char **argv){
//int fdesc = llopen(argv[1], SEND);
//exit(-1);
setbuf(stdout, NULL);
ll.timeOut = 3;
ll.sequenceNumber = 0;
ll.numTransmissions = 3;
int mode;
if(argc == 2 && !strcmp("--help", argv[1])){
printTutorial();
exit(0);
}
if(argc >= 3){
if(!strcmp(argv[2], "send"))
mode = 0;
else if(!strcmp(argv[2], "receive")){
mode = 1;
}
else{
printUsage(argv[0]);
exit(-1);
}
}
else
exit(-1);
if( (argc < 4 && mode == SEND) || (argc<3 && mode == RECEIVE) || strncmp(argv[1], "/dev/ttyS", strlen("dev/ttyS"))) {
printUsage(argv[0]);
exit(-1);
}
if((argc > 4 && mode == SEND) || (argc > 3 && mode == RECEIVE)){
int i;
for(i = 0; i < argc-(mode==SEND?4:3); i++){
if(parseParams(argv[i+(mode==SEND?4:3)]) == -1)
return -1;
}
}
int fd;
if (mode == SEND && (fd = open(argv[3], O_RDONLY)) == ENOENT){
perror("");
exit(-1);
}
int serialPort = llopen(argv[1], mode);
if (serialPort < 0){
perror("");
exit(-1);
}
int result;
if (mode == SEND){
result = sendFile(serialPort, fd, argv[3]);
}
else if (mode == RECEIVE){
result = readFile(serialPort);
}
if(result){
perror("could not transmit file");
return result;
}
printStatistics(visMode);
return 0;
}
void handleRESTRequest( const char *rq, // the full request
string url,
string& responseMsg,
int& responseCode,
vector<string>& headers // if completely empty, content-type: text/html will be added
) {
string::size_type first = url.find( "/" , 1 );
if ( first == string::npos ) {
responseCode = 400;
return;
}
string method = parseMethod( rq );
string dbname = url.substr( 1 , first - 1 );
string coll = url.substr( first + 1 );
string action = "";
map<string,string> params;
if ( coll.find( "?" ) != string::npos ) {
parseParams( params , coll.substr( coll.find( "?" ) + 1 ) );
coll = coll.substr( 0 , coll.find( "?" ) );
}
string::size_type last = coll.find_last_of( "/" );
if ( last == string::npos ) {
action = coll;
coll = "_defaultCollection";
}
else {
action = coll.substr( last + 1 );
coll = coll.substr( 0 , last );
}
for ( string::size_type i=0; i<coll.size(); i++ )
if ( coll[i] == '/' )
coll[i] = '.';
string fullns = dbname + "." + coll;
headers.push_back( (string)"x-action: " + action );
headers.push_back( (string)"x-ns: " + fullns );
headers.push_back( "Content-Type: text/plain;charset=utf-8" );
stringstream ss;
if ( method == "GET" ) {
responseCode = 200;
handleRESTQuery( fullns , action , params , responseCode , ss );
}
else if ( method == "POST" ) {
responseCode = 201;
handlePost( fullns , body( rq ) , params , responseCode , ss );
}
else {
responseCode = 400;
headers.push_back( "X_err: bad request" );
ss << "don't know how to handle a [" << method << "]";
out() << "don't know how to handle a [" << method << "]" << endl;
}
responseMsg = ss.str();
}
int main (int argc, char *argv[])
{
Params p;
int obj = -1, reader = -1, writer = -1;
int i = 0;
char number[100];
char outfilename[4096];
const char *id;
p.width =
p.height =
p.bond =
p.mode = -1;
p.id =
p.string_to_load =
p.file_to_load = NULL;
p.hydro_set =
p.query_set =
p.smarts_set = 0;
p.aromatization = NONE;
p.comment_field = NULL;
p.comment = NULL;
p.comment_name = 0;
if (argc <= 2)
USAGE();
indigoSetErrorHandler(onError, 0);
indigoSetOption("ignore-stereochemistry-errors", "on");
if (parseParams(&p, argc, argv) < 0)
return -1;
p.out_ext = OEXT_OTHER;
if (strcmp(p.outfile_ext, "mol") == 0)
p.out_ext = OEXT_MOL;
else if (strcmp(p.outfile_ext, "sdf") == 0)
p.out_ext = OEXT_SDF;
else if (strcmp(p.outfile_ext, "rxn") == 0)
p.out_ext = OEXT_RXN;
else if (strcmp(p.outfile_ext, "rdf") == 0)
p.out_ext = OEXT_RDF;
else if (strcmp(p.outfile_ext, "cml") == 0)
p.out_ext = OEXT_CML;
// guess whether to layout or render by extension
p.action = ACTION_LAYOUT;
if (p.out_ext == OEXT_OTHER) {
indigoSetOption("render-output-format", p.outfile_ext);
p.action = ACTION_RENDER;
}
// read in the input
reader = (p.file_to_load != NULL) ? indigoReadFile(p.file_to_load) : indigoReadString(p.string_to_load);
if (p.mode == MODE_SINGLE_MOLECULE) {
if (p.id != NULL)
ERROR("on single input, setting '-id' is not allowed\n");
if (p.out_ext == OEXT_RXN)
ERROR("reaction output specified for molecule input\n");
if (p.smarts_set)
obj = indigoLoadSmarts(reader);
else if (p.query_set)
obj = indigoLoadQueryMolecule(reader);
else
obj = indigoLoadMolecule(reader);
_prepare(obj, p.aromatization);
if (p.action == ACTION_LAYOUT) {
indigoLayout(obj);
if (p.out_ext == OEXT_MOL)
indigoSaveMolfileToFile(obj, p.outfile);
else
indigoSaveCmlToFile(obj, p.outfile);
} else {
_setComment(obj, &p);
renderToFile(obj, p.outfile);
}
} else if (p.mode == MODE_SINGLE_REACTION) {
if (p.id != NULL)
ERROR("on single input, setting '-id' is not allowed\n");
if (p.out_ext == OEXT_MOL)
ERROR("molecule output specified for reaction input\n");
if (p.smarts_set)
obj = indigoLoadReactionSmarts(reader);
else if (p.query_set)
obj = indigoLoadQueryReaction(reader);
else
obj = indigoLoadReaction(reader);
_prepare(obj, p.aromatization);
if (p.action == ACTION_LAYOUT) {
indigoLayout(obj);
if (p.out_ext == OEXT_CML)
indigoSaveCmlToFile(obj, p.outfile);
else
indigoSaveRxnfileToFile(obj, p.outfile);
} else {
_setComment(obj, &p);
renderToFile(obj, p.outfile);
}
} else {
int item;
int have_percent_s = (strstr(p.outfile, "%s") != NULL);
if (p.mode == MODE_MULTILINE_SMILES)
obj = indigoIterateSmiles(reader);
else if (p.mode == MODE_SDF)
obj = indigoIterateSDF(reader);
else if (p.mode == MODE_MULTIPLE_CML)
obj = indigoIterateCML(reader);
else if (p.mode == MODE_RDF)
obj = indigoIterateRDF(reader);
else {
fprintf(stderr, "internal error: wrong branch\n");
return -1;
}
if ((p.out_ext == OEXT_MOL || p.out_ext == OEXT_RXN || p.out_ext == OEXT_OTHER) && !have_percent_s)
ERROR("on multiple output, output file name must have '%%s'\n");
if (p.out_ext == OEXT_SDF || p.out_ext == OEXT_RDF ||
(p.out_ext == OEXT_CML && !have_percent_s))
{
writer = indigoWriteFile(p.outfile);
if (p.out_ext == OEXT_RDF)
indigoRdfHeader(writer);
if (p.out_ext == OEXT_CML)
indigoCmlHeader(writer);
}
i = -1;
while ((item = indigoNext(obj))) {
int rc;
++i;
if (writer > 0)
printf("saving item #%d... ", i);
else
{
if (p.id) {
if (!indigoHasProperty(item, p.id)) {
fprintf(stderr, "item #%d does not have %s, skipping\n", i, p.id);
continue;
}
id = indigoGetProperty(item, p.id);
snprintf(outfilename, sizeof(outfilename), p.outfile, id);
} else {
snprintf(number, sizeof(number), "%d", i);
snprintf(outfilename, sizeof(outfilename), p.outfile, number);
}
printf("saving %s... ", outfilename);
}
indigoSetErrorHandler(0, 0);
if (_prepare(item, p.aromatization) < 0)
{
printf("%s\n", indigoGetLastError());
indigoSetErrorHandler(onError, 0);
continue;
}
if (p.action == ACTION_LAYOUT)
{
if (indigoLayout(item) < 0)
{
printf("%s\n", indigoGetLastError());
indigoSetErrorHandler(onError, 0);
continue;
}
}
if (writer > 0) {
if (p.out_ext == OEXT_SDF)
rc = indigoSdfAppend(writer, item);
else if (p.out_ext == OEXT_RDF)
rc = indigoRdfAppend(writer, item);
else
rc = indigoCmlAppend(writer, item);
} else {
if (p.action == ACTION_LAYOUT) {
if (p.out_ext == OEXT_MOL)
rc = indigoSaveMolfileToFile(item, outfilename);
else if (p.out_ext == OEXT_RXN)
rc = indigoSaveRxnfileToFile(item, outfilename);
else
ERROR("extension unexpected");
} else {
_setComment(item, &p);
rc = indigoRenderToFile(item, outfilename);
}
}
if (rc < 0)
{
printf("%s\n", indigoGetLastError());
indigoSetErrorHandler(onError, 0);
continue;
}
indigoFree(item);
indigoSetErrorHandler(onError, 0);
printf("\n");
}
if (writer > 0)
{
if (p.out_ext == OEXT_CML)
indigoCmlFooter(writer);
indigoFree(writer);
}
}
indigoFree(reader);
indigoFree(obj);
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "cart_to_joint");
ros::NodeHandle nh;
ros::NodeHandle _nh("~");
if(!parseParams(_nh)) {
ROS_ERROR("Errors while parsing arguments.");
return 1;
}
// Initialize robot with/without orientation (variable from launch file)
mRobot = new RTKRobotArm(bOrientCtrl);
if(!mRobot->initialize(_nh)) {
ROS_ERROR("Error while loading robot");
return 1;
}
numdof = mRobot->numdof;
des_ee_ft.resize(6);
est_ee_ft.resize(6);
des_ee_stiff.resize(6);
shift_ee_ft.resize(6);
for(int i=0; i<6; ++i) {
des_ee_ft(i)=0;
est_ee_ft(i)=0;
shift_ee_ft(i)=0;
}
read_jpos.resize(numdof);
joint_lims.resize(numdof);
// TODO: need to find a better way to do this from launch files
/********** Specific for RTKRobotArm (KUKA) ****************/
/* joint_lims[0] = 150;joint_lims[1] = 107;joint_lims[2] = 150; joint_lims[3] = 107;
joint_lims[4] = 150;joint_lims[5] = 115;joint_lims[6] = 150;*/
/********** Specific for RTKRobotArm (BOXY) ****************/
joint_lims[0] = 150;joint_lims[1] = 107;joint_lims[2] = 150; joint_lims[3] = 107;
joint_lims[4] = 150;joint_lims[5] = 115;joint_lims[6] = 150;
joint_lims = joint_lims*M_PI/180.0;
mRobot->setJointLimits(joint_lims);
Eigen::VectorXd np; np.resize(numdof);
np[0] = -0.62; np[1] = 0.76; np[2] = 0.61; np[3] = -1.0;
np[4] = -0.40; np[5] = 1.4; np[6] = 1.4;
// TODO: not implemented properly yet
// mRobot->setNullPosture(np);
/*********************************************************/
if(bUseIAI) {
msg_vel_stiff_iai.stiffness.resize(numdof);
msg_vel_stiff_iai.damping.resize(numdof);
msg_vel_stiff_iai.add_torque.resize(numdof);
msg_vel_stiff_iai.velocity.resize(numdof);
} else {
msg_vel_stiff_jstate.velocity.resize(numdof);
}
if(bUseIAI) {
ROS_INFO_STREAM("Using iai_control_msgs");
pub_joints = nh.advertise<iai_control_msgs::MultiJointVelocityImpedanceCommand>(output_joints_topic, 3);
} else {
ROS_INFO_STREAM("Joints states");
// pub_joints = nh.advertise<sensor_msgs::JointState>(output_joints_topic, 3);
pub_joints = nh.advertise<kuka_fri_bridge::JointStateImpedance>(output_joints_topic, 3);
}
ros::Subscriber sub_pos = nh.subscribe<geometry_msgs::PoseStamped>(input_pose_topic, 1, cartCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_ft = nh.subscribe<geometry_msgs::WrenchStamped>(input_ft_topic, 1, ftCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_stiff = nh.subscribe<geometry_msgs::WrenchStamped>(input_stiff_topic, 1, stiffnessCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_est_ft = nh.subscribe<geometry_msgs::WrenchStamped>(input_estimate_ft_topic, 1, estFTCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_jstate = nh.subscribe<sensor_msgs::JointState>(input_joint_topic, 1, jointStateCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub = nh.subscribe("Action_State", 1, actionStateCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_ja = nh.subscribe<std_msgs::Bool>("joint_action", 1, jointActionCallback, ros::TransportHints().tcpNoDelay());
joint_vel.resize(numdof); joint_stiff.resize(numdof), joint_damp.resize(numdof);
for(int i=0; i<numdof; ++i) {
joint_vel[i] = 0.;
joint_stiff[i] = DEFAULT_JSTIFF;
joint_damp[i] = DEFAULT_JDAMP;
}
ROS_INFO("Waiting for robot joint state and FT estimate topic...");
ros::Rate r(1000);
isJointOkay = false; isFTOkay = false; isAllOkay = false;
while(ros::ok() && (!isJointOkay || (bUseForce && !isFTOkay)) ) {
// while(ros::ok()){
ros::spinOnce();
r.sleep();
}
tf::Pose tmp;
mRobot->setJoints(read_jpos);
mRobot->getEEPose(tmp);
des_ee_pose = tmp;
// Remove the residual force/torque from the future estimates. To compensate for error in ee force estimation or tool calibration
shift_ee_ft = est_ee_ft;
ROS_INFO_STREAM("Shift: "<<shift_ee_ft.transpose());
if(shift_ee_ft.norm() > ft_tol) {
ROS_WARN("Shift in EE FT estimate more than the required tolerance!!");
ROS_WARN("Either increase the tolerance or calibrate the tool better");
ros::Duration warn(2);
warn.sleep();
}
ROS_INFO("Node started");
isAllOkay = true;
// This is to ensure that we send atleast one zero velocity message before dying!
// For safety.
ros::Rate r_(1000);
while(ros::ok()) {
ros::spinOnce();
r_.sleep();
}
for(int i=0; i<numdof; ++i) {
joint_vel[i] = 0.0;
}
sendJointMessage(joint_vel, joint_stiff, joint_damp);
ros::Duration(1.0).sleep();
nh.shutdown();
return 0;
}
