void SpecificWorker::action_ChangeRoom(bool newAction)
{
static float lastX = std::numeric_limits<float>::quiet_NaN();
static float lastZ = std::numeric_limits<float>::quiet_NaN();
auto symbols = worldModel->getSymbolsMap(params, "r2", "robot");
try
{
printf("trying to get _in_ edge from %d to %d\n", symbols["robot"]->identifier, symbols["r2"]->identifier);
AGMModelEdge e = worldModel->getEdge(symbols["robot"], symbols["r2"], "in");
printf("STOP! WE ALREADY GOT THERE!\n");
stop();
return;
}
catch(...)
{
}
int32_t roomId = symbols["r2"]->identifier;
printf("room symbol: %d\n", roomId);
std::string imName = symbols["r2"]->getAttribute("imName");
printf("imName: <%s>\n", imName.c_str());
const float refX = str2float(symbols["r2"]->getAttribute("x"));
const float refZ = str2float(symbols["r2"]->getAttribute("z"));
QVec roomPose = innerModel->transformS("world", QVec::vec3(refX, 0, refZ), imName);
roomPose.print("goal pose");
// AGMModelSymbol::SPtr goalRoom = worldModel->getSymbol(str2int(params["r2"].value));
// const float x = str2float(goalRoom->getAttribute("tx"));
// const float z = str2float(goalRoom->getAttribute("tz"));
const float x = roomPose(0);
const float z = roomPose(2);
bool proceed = true;
if ( (planningState.state=="PLANNING" or planningState.state=="EXECUTING") )
{
if (abs(lastX-x)<10 and abs(lastZ-z)<10)
proceed = false;
else
printf("proceed because the coordinates differ (%f, %f), (%f, %f)\n", x, z, lastX, lastZ);
}
else
{
printf("proceed because it's stoped\n");
}
if (proceed)
{
lastX = x;
lastZ = z;
printf("changeroom to %d\n", symbols["r2"]->identifier);
go(x, z, 0, false, 0, 0, 1200);
}
else
{
}
}
void SpecificWorker::ballCentered()
{
int32_t ball = atoi(params["b"].value.c_str());
int32_t robot = atoi(params["r"].value.c_str());
try
{
const float tx = str2float(worldModel->getSymbol(ball)->getAttribute("tx"));
const float ty = str2float(worldModel->getSymbol(ball)->getAttribute("ty"));
const float tz = str2float(worldModel->getSymbol(ball)->getAttribute("tz"));
QVec poseTr = innerModel->transform("world", QVec::vec3(tx, ty, tz), "robot");
printf("gooooooo saccadic3D T=(%.2f, %.2f, %.2f)\n", poseTr(0), poseTr(1), poseTr(2));
saccadic3D(poseTr, QVec::vec3(0,-1,0));
/// Include "fixates" edge
AGMModel::SPtr newModel(new AGMModel(worldModel));
newModel->addEdgeByIdentifiers(robot, ball, "fixates");
sendModificationProposal(worldModel, newModel);
}
catch(AGMModelException &e)
{
try
{
speech_proxy->say("acho, pasó algo raro con centered", true);
}
catch(AGMModelException &e)
{
printf("acho, pasó algo raro con speech en algo raro con centered\n");
}
printf("I don't know object %d\n", ball);
}
}
bool split_xy (const std::string &coord, float &x, float &y)
{
std::string::size_type pos = coord.find(',');
if(std::string::npos==pos)
return false;
std::string val = coord.substr(0,pos);
x = str2float(val);
val = coord.substr(pos+1);
y = str2float(val);
return true;
}
static int
nmea_reader_update_time( NmeaReader* r, Token tok )
{
int hour, minute;
double seconds;
struct tm tm;
time_t fix_time;
if (tok.p + 6 > tok.end)
return -1;
if (r->utc_year <= 2000) {
r->utc_year = 1970;
r->utc_mon = 1;
r->utc_day = 1;
}
hour = str2int(tok.p, tok.p+2);
minute = str2int(tok.p+2, tok.p+4);
seconds = str2float(tok.p+4, tok.end);
tm.tm_hour = hour;
tm.tm_min = minute;
tm.tm_sec = (int) seconds;
tm.tm_year = r->utc_year - 1900;
tm.tm_mon = r->utc_mon - 1;
tm.tm_mday = r->utc_day;
fix_time = utc_mktime( &tm );
r->fix.timestamp = (long long)fix_time * 1000;
return 0;
}
static int
nmea_reader_update_timemap( NmeaReader* r,
Token systime_tok,
Token timestamp_tok)
{
int ret;
time_t timestamp;
if ( systime_tok.p >= systime_tok.end ||
timestamp_tok.p >= timestamp_tok.end)
{
r->timemap.valid = 0;
return -1;
}
ret = nmea_reader_get_timestamp(r, timestamp_tok, ×tamp);
if (ret)
{
r->timemap.valid = 0;
return ret;
}
r->timemap.valid = 1;
r->timemap.systime = str2float(systime_tok.p, systime_tok.end);
r->timemap.timestamp = (GpsUtcTime)((long long)timestamp * 1000);
return 0;
}
static int
nmea_reader_update_sv_list( GpsSvInfo* sv,
Token prn,
Token elevation,
Token azimuth,
Token snr)
{
Token tok = prn;
if(tok.p >= tok.end){
memset(sv, 0, sizeof(GpsSvInfo));
return -1;
}
sv->prn = str2int(prn.p, prn.end);
sv->snr = str2float(snr.p, snr.end);
sv->elevation = str2float(elevation.p, elevation.end);
sv->azimuth = str2float(azimuth.p, azimuth.end);
return 0;
}
static double
convert_from_hhmm( Token tok )
{
double val = str2float(tok.p, tok.end);
int degrees = (int)(floor(val) / 100);
double minutes = val - degrees*100.;
double dcoord = degrees + minutes / 60.0;
return dcoord;
}
float
scr_GetFloatDef (const char *key, float def)
{
const char *v;
check_map_init ();
v = scr_GetString (key);
return v ? str2float (v) : def;
}
void csv2floats(const std::string& istr, std::valarray<double>& vv)
{
std::vector<double> vl(0);
vl.clear(); std::string ls=istr;
int pos=0;
while( (pos = ls.find_first_of(',')) != ls.npos )
{
if(pos > 0)
{
vl.push_back(str2float(ls.substr(0,pos)));
}
ls=ls.substr(pos+1);
}
if(ls.length() > 0)
{
vl.push_back(str2float(ls));
}
vv.resize(vl.size()); //copies onto a valarray
for (int k=0; k<vl.size(); k++) vv[k]=vl[k];
}
ColorSelectorMerger::ColorSelectorMerger(std::vector<std::pair<properties::tagType, properties::iterator> > tags,
std::map<std::string, streamRecord*>& outStreamRecords,
std::vector<std::map<std::string, streamRecord*> >& inStreamRecords,
properties* props) :
Merger(advance(tags), outStreamRecords, inStreamRecords, props)
{
if(props==NULL) props = new properties();
this->props = props;
map<string, string> pMap;
properties::tagType type = streamRecord::getTagType(tags);
if(type==properties::unknownTag) { cerr << "ERROR: inconsistent tag types when merging Trace!"<<endl; exit(-1); }
if(type==properties::enterTag) {
assert(tags.size()>0);
vector<string> names = getNames(tags); assert(allSame<string>(names));
assert(*names.begin() == "colorSelector");
// Merge the selector IDs along all the streams
int mergedSelID = streamRecord::mergeIDs("colorSelector", "selID", pMap, tags, outStreamRecords, inStreamRecords);
pMap["selID"] = txt()<<mergedSelID;
pMap["startR"] = txt()<<vAvg(str2float(getValues(tags, "startR")));
pMap["endR"] = txt()<<vAvg(str2float(getValues(tags, "endR")));
pMap["startG"] = txt()<<vAvg(str2float(getValues(tags, "startG")));
pMap["endG"] = txt()<<vAvg(str2float(getValues(tags, "endG")));
pMap["startB"] = txt()<<vAvg(str2float(getValues(tags, "startB")));
pMap["endB"] = txt()<<vAvg(str2float(getValues(tags, "endB")));
}
props->add("colorSelector", pMap);
}
void SpecificWorker::updateSymbolWithTag(AGMModelSymbol::SPtr symbol, const AprilTagModel &tag)
{
innerModel->updateTransformValues("trTag", tag.tx, tag.ty, tag.tz, tag.rx+M_PIl, tag.ry, tag.rz);
QVec T = innerModel->transform("robot", QVec::vec3(0,0,0), "trTag");
QVec R = innerModel->getRotationMatrixTo("trTag", "robot").extractAnglesR_min();
//threshold to update set to 20 mm and 0.1 rad
if ( fabs(str2float((symbol->attributes["tx"])) - T(0)) > 20 ||
fabs(str2float((symbol->attributes["ty"])) - T(1)) > 20 ||
fabs(str2float((symbol->attributes["tz"])) - T(2)) > 20 ||
fabs(str2float((symbol->attributes["rx"])) - R(0)) > 0.1 ||
fabs(str2float((symbol->attributes["ry"])) - R(1)) > 0.1 ||
fabs(str2float((symbol->attributes["rz"])) - R(2)) > 0.1 )
{
symbol->attributes["tx"] = float2str(T(0));
symbol->attributes["ty"] = float2str(T(1));
symbol->attributes["tz"] = float2str(T(2));
symbol->attributes["rx"] = float2str(R(0));
symbol->attributes["ry"] = float2str(R(1));
symbol->attributes["rz"] = float2str(R(2));
RoboCompAGMWorldModel::Node symbolICE;
AGMModelConverter::fromInternalToIce(symbol, symbolICE);
agmagenttopic->update(symbolICE);
}
}
/** Loading file that contains the objectives and
* final collumn has the identificator
*/
owner_file_t * loading_owner_file_solution_file_name_at_ending(const int *num_solutions_r, const int *numobj_r, const char *path_file_name){
FILE * solution_file;
char *line;
char *line_splited;
int sol;
owner_file_t * solutions_aux;
line = Malloc(char, MAX_LINE_SOLUTION_FILE);
//Alocating Solution
solutions_aux = allocate_file_t(num_solutions_r, numobj_r);
//Reading file and set values of objective
sol = -1;
solution_file = open_file(path_file_name, fREAD);
//Removing first line that is collumn
fgets(line,MAX_LINE_SOLUTION_FILE,solution_file);
while ( fgets(line,MAX_LINE_SOLUTION_FILE,solution_file) != NULL){
sol = sol + 1;
//Obtaing index collumn
line_splited = strtok (line,"\t");
//Objective 0
trim(line_splited);
solutions_aux[sol].obj_values[0] = str2float(line_splited);
//Objective 1
line_splited = strtok (NULL,"\t");
trim(line_splited);
solutions_aux[sol].obj_values[1] = str2float(line_splited);
//identificator
line_splited = strtok (NULL,"\t");
remove_character(line_splited, '\n');
trim(line_splited);
strcpy(solutions_aux[sol].file_name, line_splited);
}
fclose(solution_file);
free(line);
return solutions_aux;
}
static int
nmea_reader_update_bearing( NmeaReader* r,
Token bearing )
{
double alt;
Token tok = bearing;
if (tok.p >= tok.end)
return -1;
r->fix.flags |= GPS_LOCATION_HAS_BEARING;
r->fix.bearing = str2float(tok.p, tok.end);
return 0;
}
/*---------------------------------------------------------------------------*/
static void
broadcast_recv(struct broadcast_conn *c, const rimeaddr_t *from)
{
char * msg = strdup((char *)packetbuf_dataptr()), * x, * y;
if(msg[0] == 'N'){
printf("NEIGHBOR %d\n", from->u8[0]);
return;
}
numofneighs++;
point = (Point *) malloc(sizeof(Point));
x = strtok(msg, "#");
y = strtok(NULL, "#");
point->id = from->u8[0];
point->x = str2float(x);
point->y = str2float(y);
free(x);
free(y);
free(msg);
//consider points for delaunay graph calculation.
point->rssi = packetbuf_attr(PACKETBUF_ATTR_RSSI) - 45;
addREDELCApoint(point);
}
static int
nmea_reader_update_speed( NmeaReader* r,
Token speed )
{
double alt;
Token tok = speed;
if (tok.p >= tok.end)
return -1;
r->fix.flags |= GPS_LOCATION_HAS_SPEED;
r->fix.speed = str2float(tok.p, tok.end);
return 0;
}
int main(int argc, char *argv[])
{
float A, B, C, correct1;
int t_success=0, t_count=0;
char inA[70],inB[70],outC[70],corC[70];
if( sizeof(long) != sizeof(float) )
{
puts("error sizeof long != sizeof float");
return -1;
}
while(scanf("%5s",inA) == 1)
{
if(strcmp(inA,"TEST1") == 0)
{
scanf("%s %s %s", inA, inB, outC);
A = str2float(inA);
B = str2float(inB);
C = str2float(outC);
correct1 = A + B;
float2str(correct1, corC);
}
else if(strcmp(inA,"TEST2") == 0)
{
scanf("%s %s %s %s", inA, inB, outC, corC);
A = str2float(inA);
B = str2float(inB);
C = str2float(outC);
correct1 = str2float(corC);
}
else
{
while(getchar() != '\n');//skip until end of the line
continue;
}
if(! fequal(corC, outC))
{
printf("Test #%d failed: %e + %e = %e != %e\n", t_count+1, A, B, correct1, C);
printf(" A %s\n +B %s\n == %s\n != %s\n", inA, inB, corC, outC);
}
else
{
//printf("Test OK! (%.2f + %.2f = %.2f)\n", A, B, C);
t_success++;
}
t_count++;
}
printf("Succeded %d tests, Failed %d tests\n", t_success, t_count - t_success);
printf("Sucess: %.2lf%%\n",100 * t_success / (double)t_count );
return 0;
}
static int
nmea_reader_update_altitude( NmeaReader* r,
Token altitude,
Token units )
{
double alt;
Token tok = altitude;
if (tok.p >= tok.end)
return -1;
r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
r->fix.altitude = str2float(tok.p, tok.end);
return 0;
}
static int
nmea_reader_update_speed( NmeaReader* r,
Token speed )
{
double alt;
Token tok = speed;
if (tok.p >= tok.end)
return -1;
r->fix.flags |= GPS_LOCATION_HAS_SPEED;
r->fix.speed = str2float(tok.p, tok.end);
r->fix.speed *= 0.514444; // fix for Speed Unit form Knots to Meters per Second
return 0;
}
void Graph::set_colorlines(const std::vector<std::string> &_colorlines) {
this->colorlines = _colorlines;
std::string delimiter = "#";
for(std::vector<std::string>::const_iterator it = this->colorlines.begin();
it != this->colorlines.end(); ++it) {
std::string value = it->substr(0, it->find(delimiter));
std::string rgb = it->substr(it->find(delimiter) + 1);
this->colorline_values.push_back( str2float(value) );
Color clr(hex2int(rgb.substr(0,2)),
hex2int(rgb.substr(2,2)),
hex2int(rgb.substr(4,2)) );
this->colorline_colors.push_back(clr);
}
}
void SpecificWorker::updateWristPose()
{
// Make sure we have the robot in the model, otherwise there's nothing to do yet...
int32_t robotId = worldModel->getIdentifierByType("robot");
if (robotId == -1)
{
return;
}
AGMModelSymbol::SPtr robot = worldModel->getSymbol(robotId);
// Set current T and R
QVec T = innerModel->transform("robot", QVec::vec3(0,0,0), "arm_right_8");
QVec R = innerModel->getRotationMatrixTo("arm_right_8", "robot").extractAnglesR_min();
// Set back T and R
QVec T_back, R_back;
bool force = false;
try
{
T_back = QVec::vec3(
str2float(robot->attributes["rightwrist_tx"]),
str2float(robot->attributes["rightwrist_ty"]),
str2float(robot->attributes["rightwrist_tz"]));
R_back = QVec::vec3(
str2float(robot->attributes["rightwrist_rx"]),
str2float(robot->attributes["rightwrist_ry"]),
str2float(robot->attributes["rightwrist_rz"]));
}
catch(...)
{
printf("exception in updateWristPose %d\n", __LINE__);
force = true;
}
#warning These thresholds should be set in the config file!!!
#warning These thresholds should be set in the config file!!!
#warning These thresholds should be set in the config file!!!
#warning These thresholds should be set in the config file!!!
if ( force or (T-T_back).norm2()>15 or (R-R_back).norm2()>0.05)
{
robot->attributes["rightwrist_tx"] = float2str(T(0));
robot->attributes["rightwrist_ty"] = float2str(T(1));
robot->attributes["rightwrist_tz"] = float2str(T(2));
robot->attributes["rightwrist_rx"] = float2str(R(0));
robot->attributes["rightwrist_ry"] = float2str(R(1));
robot->attributes["rightwrist_rz"] = float2str(R(2));
RoboCompAGMWorldModel::Node nodeDst;
AGMModelConverter::fromInternalToIce(robot, nodeDst);
agmagenttopic->update(nodeDst);
}
}
static int
nmea_reader_update_accuracy( NmeaReader* r,
Token accuracy )
{
double acc;
Token tok = accuracy;
if (tok.p >= tok.end)
return -1;
r->fix.accuracy = str2float(tok.p, tok.end);
if (r->fix.accuracy == 99.99){
return 0;
}
r->fix.flags |= GPS_LOCATION_HAS_ACCURACY;
return 0;
}
/*******************************************************************
* Function Name: LTSNetwork
* Description: constructor
********************************************************************/
LTSNetwork::LTSNetwork( const string &name )
: Atomic( name )
, peer_online( addInputPort( "peer_online" ) )
, peer_offline( addInputPort( "peer_offline" ) )
, peer_connect( addInputPort( "peer_connect" ) )
, peer_disconnect( addInputPort( "peer_disconnect" ) )
, inroute( addInputPort( "inroute" ) )
, route_out( addOutputPort( "route_out" ) )
, out_connect( addOutputPort( "out_connect" ) )
, out_disconnect( addOutputPort( "out_disconnect" ) )
{
//create a new Graph.
thegraph= new GraphInt();
currenttimefloat=0; // start the timer at 0.
//get probability distribution for the network delay
try
{
dist = Distribution::create( MainSimulator::Instance().getParameter( description(), "distribution" ) );
MASSERT( dist );
for ( register int i = 0 ; i < dist->varCount() ; i++ )
{
string parameter( MainSimulator::Instance().getParameter( description(), dist->getVar(i) ) ) ;
dist->setVar( i, str2float( parameter ) ) ;
}
} catch( InvalidDistribution &e )
{
e.addText( "The model " + description() + " has distribution problems!" ) ;
e.print(cerr);
MTHROW( e ) ;
} catch( MException &e )
{
MTHROW( e ) ;
}
}
/** Loading a solution file.
* Returns: an array of solution
* Impotant: This array of solution need to be desallocated
*/
_2PG_CARTESIAN_EXPORT
solution_t * loading_file_solutions(int *num_solutions_r,
int *numobj_r, const char *path_file_name){
FILE * solution_file;
char *line;
char *line_splited;
int sol;
solution_t * solutions_aux;
line = Malloc(char, MAX_LINE_SOLUTION);
//Getting initial information
initialize_for_reading_file(num_solutions_r, numobj_r, path_file_name);
//Alocating Solution
solutions_aux = allocate_solution(num_solutions_r, numobj_r);
//Reading file and set values of objective
sol = -1;
solution_file = open_file(path_file_name, fREAD);
//Removing first line that is collumn
fgets(line,MAX_LINE_SOLUTION,solution_file);
while ( fgets(line,MAX_LINE_SOLUTION,solution_file) != NULL){
sol = sol + 1;
//Removing index collumn
line_splited = strtok (line,"\t");
//Setting number of objectives
solutions_aux[sol].num_obj = *numobj_r;
for (int ob = 0; ob < *numobj_r; ob++){
line_splited = strtok (NULL,"\t");
trim(line_splited);
solutions_aux[sol].obj_values[ob] = str2float(line_splited);//str2double(line_splited);
}
}
fclose(solution_file);
free(line);
return solutions_aux;
}
//保存输入数据
ErrorStatus SavePutinChars( TYPE_SAVE_TypeDef save_type, char *source, int32_t *target_int,
float *target_float, char *target_char, uint8_t source_cnt, uint8_t *target_cnt )
{
if ( source_cnt )
{
source[source_cnt] = 0;
switch ( save_type )
{
case TYPE_INT: //保存为整型数据
FindPutcharIntIllegar(&source_cnt,source);
*target_int = ustrtoul(source,0,10);
break;
case TYPE_FLOAT: //保存为浮点型数据
*target_float = str2float(source);
break;
case TYPE_CHAR: //保存为字符型数据
chartochar(source_cnt,source,target_char);
break;
default:
break;
}
*target_cnt = source_cnt;
return SUCCESS;
}
else
{
*target_cnt = 0;
return ERROR;
}
}
static int nmea_reader_get_timestamp(NmeaReader* r, Token tok, time_t *timestamp)
{
int hour, minute;
double seconds;
struct tm tm;
time_t ttime;
if (tok.p + 6 > tok.end)
return -1;
if (r->utc_year < 0) {
return -1;
}
hour = str2int(tok.p, tok.p+2);
minute = str2int(tok.p+2, tok.p+4);
seconds = str2float(tok.p+4, tok.end);
tm.tm_hour = hour;
tm.tm_min = minute;
tm.tm_sec = (int) seconds;
tm.tm_year = r->utc_year - 1900;
tm.tm_mon = r->utc_mon - 1;
tm.tm_mday = r->utc_day;
tm.tm_isdst = -1;
// D("h: %d, m: %d, s: %d", tm.tm_hour, tm.tm_min, tm.tm_sec);
// D("Y: %d, M: %d, D: %d", tm.tm_year, tm.tm_mon, tm.tm_mday);
nmea_reader_update_utc_diff(r);
ttime = mktime( &tm );
*timestamp = ttime - r->utc_diff;
return 0;
}
static int
nmea_reader_update_time( NmeaReader* r, Token tok )
{
int hour, minute;
double seconds;
struct tm tm;
time_t fix_time;
if (tok.p + 6 > tok.end)
return -1;
if (r->utc_year < 0) {
// no date yet, get current one
time_t now = time(NULL);
gmtime_r( &now, &tm );
r->utc_year = tm.tm_year + 1900;
r->utc_mon = tm.tm_mon + 1;
r->utc_day = tm.tm_mday;
}
hour = str2int(tok.p, tok.p+2);
minute = str2int(tok.p+2, tok.p+4);
seconds = str2float(tok.p+4, tok.end);
tm.tm_hour = hour;
tm.tm_min = minute;
tm.tm_sec = (int) seconds;
tm.tm_year = r->utc_year - 1900;
tm.tm_mon = r->utc_mon - 1;
tm.tm_mday = r->utc_day;
tm.tm_isdst = -1;
fix_time = mktime( &tm ) + r->utc_diff;
r->fix.timestamp = (long long)fix_time * 1000;
return 0;
}
//Update object position
void SpecificWorker::RCIS_update_object(RoboCompAGMWorldModel::Node &node)
{
try
{
RoboCompInnerModelManager::Pose3D pose;
pose.rx = str2float(node.attributes["rx"]);
pose.ry = str2float(node.attributes["ry"]);
pose.rz = str2float(node.attributes["rz"]);
pose.x = str2float(node.attributes[std::string("tx") ]);
pose.y = str2float(node.attributes[std::string("ty") ]);
pose.z = str2float(node.attributes[std::string("tz") ]);
printf("Rx: %s\n",std::string(node.attributes[std::string("rx")]).c_str() );
printf("Ry: %s\n",std::string(node.attributes[std::string("ry")]).c_str());
printf("Rz: %s\n",std::string(node.attributes[std::string("rz")]).c_str());
printf("Tx: %s\n",std::string(node.attributes[std::string("tx")]).c_str());
printf("Ty: %s\n",std::string(node.attributes[std::string("ty")] ).c_str());
printf("Tz: %s\n",std::string(node.attributes[std::string("tz")]).c_str());
QString ident = QString::fromStdString(node.nodeType);
ident += "_";
ident += QString::number(node.nodeIdentifier);
printf("UPDATED: %s", ident.toStdString().c_str());
innermodelmanager_proxy->setPoseFromParent(ident.toStdString()+"_T", pose);
//innermodelmanager_proxy->setPoseFromParent("test_T", pose);
printf("UPDATED: %s", ident.toStdString().c_str());
}
catch (RoboCompInnerModelManager::InnerModelManagerError e)
{
if (e.err != RoboCompInnerModelManager::NonExistingNode)
qFatal("%s", e.text.c_str());
}
catch (...)
{
printf("SHIT %s:%d\n", __FILE__, __LINE__);
}
}
static void
nmea_reader_parse( NmeaReader* r )
{
/* we received a complete sentence, now parse it to generate
* a new GPS fix...
*/
NmeaTokenizer tzer[1];
Token tok;
if (r->pos < 9) {
return;
}
if (gps_state->callbacks.nmea_cb) {
struct timeval tv;
unsigned long long mytimems;
gettimeofday(&tv,NULL);
mytimems = tv.tv_sec * 1000 + tv.tv_usec / 1000;
gps_state->callbacks.nmea_cb(mytimems, r->in, r->pos);
}
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#ifdef GPS_DEBUG_TOKEN
{
int n;
D("Found %d tokens", tzer->count);
for (n = 0; n < tzer->count; n++) {
Token tok = nmea_tokenizer_get(tzer,n);
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
}
}
#endif
tok = nmea_tokenizer_get(tzer, 0);
if (tok.p + 5 > tok.end) {
/* for $PUNV sentences */
if ( !memcmp(tok.p, "PUNV", 4) ) {
Token tok_cfg = nmea_tokenizer_get(tzer,1);
if (!memcmp(tok_cfg.p, "CFG_R", 5)) {
} else if ( !memcmp(tok_cfg.p, "QUAL", 4) ) {
Token tok_sigma_x = nmea_tokenizer_get(tzer, 3);
if (tok_sigma_x.p[0] != ',') {
Token tok_accuracy = nmea_tokenizer_get(tzer, 10);
nmea_reader_update_accuracy(r, tok_accuracy);
}
} else if (!memcmp(tok_cfg.p, "TIMEMAP", 7))
{
Token systime = nmea_tokenizer_get(tzer, 8); // system time token
Token timestamp = nmea_tokenizer_get(tzer, 2); // UTC time token
nmea_reader_update_timemap(r, systime, timestamp);
}
}else{
}
return;
}
if ( !memcmp(tok.p, "GPG", 3) ) //GPGSA,GPGGA,GPGSV
bGetFormalNMEA = 1;
// ignore first two characters.
tok.p += 2;
if ( !memcmp(tok.p, "GGA", 3) ) {
// GPS fix
Token tok_fixstaus = nmea_tokenizer_get(tzer,6);
if (tok_fixstaus.p[0] > '0') {
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,2);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
Token tok_longitude = nmea_tokenizer_get(tzer,4);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
Token tok_altitude = nmea_tokenizer_get(tzer,9);
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
}
} else if ( !memcmp(tok.p, "GLL", 3) ) {
Token tok_fixstaus = nmea_tokenizer_get(tzer,6);
if (tok_fixstaus.p[0] == 'A') {
Token tok_latitude = nmea_tokenizer_get(tzer,1);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,2);
Token tok_longitude = nmea_tokenizer_get(tzer,3);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_time = nmea_tokenizer_get(tzer,5);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
}
} else if ( !memcmp(tok.p, "GSA", 3) ) {
Token tok_fixStatus = nmea_tokenizer_get(tzer, 2);
int i;
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1') {
r->sv_status.used_in_fix_mask = 0ul;
for (i = 3; i <= 14; ++i){
Token tok_prn = nmea_tokenizer_get(tzer, i);
int prn = str2int(tok_prn.p, tok_prn.end);
/* only available for PRN 1-32 */
if ((prn > 0) && (prn < 33)){
r->sv_status.used_in_fix_mask |= (1ul << (prn-1));
r->sv_status_changed = 1;
/* mark this parameter to identify the GSA is in fixed state */
r->gsa_fixed = 1;
}
}
}else {
if (r->gsa_fixed == 1) {
r->sv_status.used_in_fix_mask = 0ul;
r->sv_status_changed = 1;
r->gsa_fixed = 0;
}
}
} else if ( !memcmp(tok.p, "GSV", 3) ) {
Token tok_noSatellites = nmea_tokenizer_get(tzer, 3);
int noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);
if (noSatellites > 0) {
Token tok_noSentences = nmea_tokenizer_get(tzer, 1);
Token tok_sentence = nmea_tokenizer_get(tzer, 2);
int sentence = str2int(tok_sentence.p, tok_sentence.end);
int totalSentences = str2int(tok_noSentences.p, tok_noSentences.end);
int curr;
int i;
if (sentence == 1) {
r->sv_status_changed = 0;
r->sv_status.num_svs = 0;
}
curr = r->sv_status.num_svs;
i = 0;
while (i < 4 && r->sv_status.num_svs < noSatellites){
Token tok_prn = nmea_tokenizer_get(tzer, i * 4 + 4);
Token tok_elevation = nmea_tokenizer_get(tzer, i * 4 + 5);
Token tok_azimuth = nmea_tokenizer_get(tzer, i * 4 + 6);
Token tok_snr = nmea_tokenizer_get(tzer, i * 4 + 7);
r->sv_status.sv_list[curr].prn = str2int(tok_prn.p, tok_prn.end);
r->sv_status.sv_list[curr].elevation = str2float(tok_elevation.p, tok_elevation.end);
r->sv_status.sv_list[curr].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
r->sv_status.sv_list[curr].snr = str2float(tok_snr.p, tok_snr.end);
r->sv_status.num_svs += 1;
curr += 1;
i += 1;
}
if (sentence == totalSentences) {
r->sv_status_changed = 1;
}
}
} else if ( !memcmp(tok.p, "RMC", 3) ) {
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
if (tok_fixStatus.p[0] == 'A')
{
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,3);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_longitude = nmea_tokenizer_get(tzer,5);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
Token tok_speed = nmea_tokenizer_get(tzer,7);
Token tok_bearing = nmea_tokenizer_get(tzer,8);
Token tok_date = nmea_tokenizer_get(tzer,9);
nmea_reader_update_date( r, tok_date, tok_time );
nmea_reader_update_latlong( r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0] );
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else if ( !memcmp(tok.p, "VTG", 3) ) {
Token tok_fixStatus = nmea_tokenizer_get(tzer,9);
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != 'N')
{
Token tok_bearing = nmea_tokenizer_get(tzer,1);
Token tok_speed = nmea_tokenizer_get(tzer,5);
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else if ( !memcmp(tok.p, "ZDA", 3) ) {
Token tok_time;
Token tok_year = nmea_tokenizer_get(tzer,4);
if (tok_year.p[0] != '\0') {
Token tok_day = nmea_tokenizer_get(tzer,2);
Token tok_mon = nmea_tokenizer_get(tzer,3);
nmea_reader_update_cdate( r, tok_day, tok_mon, tok_year );
}
tok_time = nmea_tokenizer_get(tzer,1);
if (tok_time.p[0] != '\0') {
nmea_reader_update_time(r, tok_time);
}
} else {
tok.p -= 2;
}
if (!gps_state->first_fix &&
gps_state->init == STATE_INIT &&
r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
ath_send_ni_notification(r);
if (gps_state->callbacks.location_cb) {
gps_state->callbacks.location_cb( &r->fix );
r->fix.flags = 0;
}
gps_state->first_fix = 1;
}
}
void readObj(const std::string &fname,
std::vector<float> &data)
{
std::vector<float> points;
std::vector<float> normals;
std::vector<float> textures;
std::ifstream f(fname.c_str());
while (f) {
std::string line;
getline(f, line);
if (f) {
std::vector<std::string> splitLine;
boost::split(splitLine, line, boost::is_any_of(" "), boost::algorithm::token_compress_on);
if (splitLine[0] == "v") {
points.push_back(str2float(splitLine[1]));
points.push_back(str2float(splitLine[2]));
points.push_back(str2float(splitLine[3]));
} else if (splitLine[0] == "vn") {
normals.push_back(str2float(splitLine[1]));
normals.push_back(str2float(splitLine[2]));
normals.push_back(str2float(splitLine[3]));
} else if (splitLine[0] == "vt") {
// texture coordinates are not used at all
textures.push_back(boost::lexical_cast<float>(splitLine[1]));
textures.push_back(boost::lexical_cast<float>(splitLine[2]));
if (splitLine.size()>3) textures.push_back(boost::lexical_cast<float>(splitLine[3]));
} else if (splitLine[0] == "f") {
//std::vector<boost::tuple<int, int, int> > face;
//std::vector<int> face;
for (unsigned i = 1; i < splitLine.size(); ++i) {
std::vector<std::string> faceLine;
boost::split(faceLine, splitLine[i], boost::is_any_of("/"), boost::algorithm::token_compress_off);
int v, t, n;
v = boost::lexical_cast<int>(faceLine[0]);
if (faceLine[1] != "") {
t = boost::lexical_cast<int>(faceLine[1]);
} else {
t = -1;
}
if (faceLine[2] != "") {
n = boost::lexical_cast<int>(faceLine[2]);
} else {
n = -1;
}
//face.push_back(boost::make_tuple<int, int, int>(v, t, n));
//faces.push_back(v - 1);
data.push_back(points[(v-1)*3]);
data.push_back(points[(v-1)*3 + 1]);
data.push_back(points[(v-1)*3 + 2]);
data.push_back(normals[(n-1)*3]);
data.push_back(normals[(n-1)*3 + 1]);
data.push_back(normals[(n-1)*3 +2]);
}
//faces.push_back(face);
} else if (splitLine[0] == "#") {
// ignore comments
}else {
std::cerr << "ERROR in obj file: unknown line: " << line << "\n";
// go on with fingers crossed
//return;
}
}
}
}
void SpecificWorker::RCIS_addObjectNode(RoboCompAGMWorldModel::Node node)
{
try
{
RoboCompInnerModelManager::Pose3D pose, pose2;
pose.x = str2float(node.attributes["tx"]);
pose.y = str2float(node.attributes["ty"]);
pose.z = str2float(node.attributes["tz"]);
pose.rx = str2float(node.attributes["rx"]);
pose.ry = str2float(node.attributes["ry"]);
pose.rz = str2float(node.attributes["rz"]);
printf("Rx: %s\n",std::string(node.attributes[std::string("rx")]).c_str() );
printf("Ry: %s\n",std::string(node.attributes[std::string("ry")]).c_str());
printf("Rz: %s\n",std::string(node.attributes[std::string("rz")]).c_str());
printf("Tx: %s\n",std::string(node.attributes[std::string("tx")]).c_str());
printf("Ty: %s\n",std::string(node.attributes[std::string("ty")] ).c_str());
printf("Tz: %s\n",std::string(node.attributes[std::string("tz")]).c_str());
pose2.x = pose2.y = pose2.z = 0;
pose2.rx = pose2.ry = pose2.rz = 0;
QString ident = QString::fromStdString(node.nodeType);
ident += "_";
ident += QString::number(node.nodeIdentifier);
//add the mesh
RoboCompInnerModelManager::meshType mesh;
//mesh.pose = pose;
mesh.pose.x = 0;
mesh.pose.y = 0;
mesh.pose.z = 0;
mesh.pose.rx = 0;
mesh.pose.ry = 0;
mesh.pose.rz = 0;
mesh.render = 0;
int32_t id = str2int(node.attributes["id"]);
if (id == 0)
{
printf("mesa!\n");
mesh.meshPath = "/home/robocomp/robocomp/files/osgModels/mobiliario/mesa_redonda.osg";
mesh.pose.z = 800;
mesh.scaleX = 100;
mesh.scaleY = 100; // <--- A 674mm radius table has a scale of "100"
mesh.scaleZ = 100; // <--- A 800mm height table has a scale of "100"
}
else if (id == 2)
{
printf("taza!\n");
mesh.meshPath = "/home/robocomp/robocomp/files/osgModels/mobiliario/taza.osg";
pose2.z = 160; // La x va claramente a la derecha
mesh.scaleX = 120;
mesh.scaleY = 120;
mesh.scaleZ = 120;
}
else
{
printf("unknown!\n");
mesh.meshPath = "/home/robocomp/robocomp/files/osgModels/basics/sphere.ive";
}
// Add the transofrm
innermodelmanager_proxy->addTransform(ident.toStdString()+"_T", "static", "robot", pose);
innermodelmanager_proxy->addTransform(ident.toStdString()+"_T2", "static", ident.toStdString()+"_T", pose2);
innermodelmanager_proxy->addMesh(ident.toStdString(), ident.toStdString()+"_T2", mesh);
printf("ADDED: %s", ident.toStdString().c_str());
}
catch (InnerModelManagerError e)
{
if (e.err != NodeAlreadyExists)
qFatal("%s", e.text.c_str());
}
catch (...)
{
qFatal("Can't connect to RCIS: %s:%d\n", __FILE__, __LINE__);
}
}
