QBic::Info QBic::parseOutput(const QByteArray &ba) const
{
Info info;
const QStringList source = QString::fromLatin1(ba).split("\n\n");
foreach(QString str, source) {
QStringList entry = str.split('\n');
if (entry.count() < 2)
continue;
if (entry.at(0).startsWith("Class ")) {
const QString className = entry.at(0).mid(6);
if (isBlacklisted(className))
continue;
QRegExp rx("size=(\\d+)");
if (rx.indexIn(entry.at(1)) == -1) {
qWarning("Could not parse class information for className %s", className.toLatin1().constData());
continue;
}
info.classSizes[className] = rx.cap(1).toInt();
} else if (entry.at(0).startsWith("Vtable for ")) {
const QString className = entry.at(0).mid(11);
if (isBlacklisted(className))
continue;
info.classVTables[className] = normalizedVTable(entry);
}
}
void FSWSAssetBlacklist::addNewItemToBlacklist(const LLUUID& id, const std::string& name, const std::string& region, LLAssetType::EType type, bool save)
{
if (isBlacklisted(id, type))
{
return;
}
LLDate curdate = LLDate(time_corrected());
std::string input_date = curdate.asString();
input_date.replace(input_date.find("T"), 1, " ");
input_date.resize(input_date.size() - 1);
LLSD data;
data["asset_name"] = name;
data["asset_region"] = region;
data["asset_type"] = type;
data["asset_date"] = input_date;
addNewItemToBlacklistData(id, data, save);
}
void Debugger::pauseIfNeeded(CallFrame* callFrame)
{
VM& vm = callFrame->vm();
auto scope = DECLARE_THROW_SCOPE(vm);
if (m_isPaused)
return;
if (m_suppressAllPauses)
return;
intptr_t sourceID = DebuggerCallFrame::sourceIDForCallFrame(m_currentCallFrame);
if (isBlacklisted(sourceID))
return;
DebuggerPausedScope debuggerPausedScope(*this);
bool pauseNow = m_pauseAtNextOpportunity;
pauseNow |= (m_pauseOnCallFrame == m_currentCallFrame);
bool didPauseForStep = pauseNow;
bool didHitBreakpoint = false;
Breakpoint breakpoint;
TextPosition position = DebuggerCallFrame::positionForCallFrame(m_currentCallFrame);
pauseNow |= didHitBreakpoint = hasBreakpoint(sourceID, position, &breakpoint);
m_lastExecutedLine = position.m_line.zeroBasedInt();
if (!pauseNow)
return;
clearNextPauseState();
// Make sure we are not going to pause again on breakpoint actions by
// reseting the pause state before executing any breakpoint actions.
TemporaryPausedState pausedState(*this);
JSGlobalObject* vmEntryGlobalObject = callFrame->vmEntryGlobalObject();
if (didHitBreakpoint) {
handleBreakpointHit(vmEntryGlobalObject, breakpoint);
// Note that the actions can potentially stop the debugger, so we need to check that
// we still have a current call frame when we get back.
if (!m_currentCallFrame)
return;
if (breakpoint.autoContinue) {
if (!didPauseForStep)
return;
didHitBreakpoint = false;
} else
m_pausingBreakpointID = breakpoint.id;
}
{
PauseReasonDeclaration reason(*this, didHitBreakpoint ? PausedForBreakpoint : m_reasonForPause);
handlePause(vmEntryGlobalObject, m_reasonForPause);
RELEASE_ASSERT(!scope.exception());
}
m_pausingBreakpointID = noBreakpointID;
if (!m_pauseAtNextOpportunity && !m_pauseOnCallFrame) {
setSteppingMode(SteppingModeDisabled);
m_currentCallFrame = nullptr;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, std::string("behavior"));
kobuki_msgs::Sound sound;
tf::TransformListener tfL;
ros::NodeHandle n;
Pid pid;
ros::Subscriber robotposesub = n.subscribe("/MCLRobotica_pos", 1000, &poseCB);
ros::Publisher cmdpub = n.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1000);
ros::Publisher sonido = n.advertise<kobuki_msgs::Sound>("mobile_base/commands/sound",1);
ros::Subscriber lost_sub = n.subscribe("/MCLRobotica_lost", 1000, &lostCB);
ros::Subscriber notlost_sub = n.subscribe("/MCLRobotica_notlost", 1000, ¬lostCB);
// ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1000);
ros::Rate loop_rate(10);
int count = 0;
int state = SEARCH;
std::string target;
std::string blacklist[NUMBOLAS];
int bolas = 0;
lastPose.pose.position.z = -1.0;
geometry_msgs::Twist cmd;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
cmd.angular.x = 0.0;
cmd.angular.y = 0.0;
cmd.angular.z = 0.0;
cmd.linear.x = 0.0;
float angle2goal;
while (ros::ok())
{
if(bolas == 3)
break;
switch(state){
case SEARCH:
{
target = "A";
std::vector<std::string> frameList;
tfL.getFrameStrings(frameList);
std::vector<std::string>::iterator it;
float dmin = 99.9;
for (it = frameList.begin(); it != frameList.end(); ++it) {
std::string frame = *it;
tf::StampedTransform BL2B;
try {
tfL.lookupTransform("base_link", frame,
ros::Time::now(), BL2B);
if (isPrefix("ball_", frame) && !isBlacklisted(frame, blacklist, bolas)) {
int d = getDistanceTo(frame);
if(dmin > d){
dmin = d;
target = frame;
}
state = GOTOBALL;
}
} catch (tf::TransformException & ex) {
;//ROS_WARN("%s", ex.what());
}
}
if(target.compare("A")==0)
std::cout<<"NADA"<<std::endl;
else
std::cout<<"TARGET = "<<target<<std::endl;
cmd.linear.x = 0.2;
if(cmd.angular.z <= 0.4)
cmd.angular.z = cmd.angular.z + 0.01;
break;
}
case GOTOBALL:
{
tf::StampedTransform BL2B;
try{
tfL.lookupTransform("base_link", target,
ros::Time::now() - ros::Duration(1.0), BL2B);
}catch(tf::TransformException & ex) {
ROS_WARN("LA HE PERDIDO: %s", ex.what());
state = SEARCH;
cmd.angular.z = 0.0;
break;
}
float ro;
ro = sqrt((BL2B.getOrigin().x())*(BL2B.getOrigin().x()) + (BL2B.getOrigin().y())*(BL2B.getOrigin().y()));
double roll, pitch, yaw;
float v,w, usoPid;
Pid velDeGiro;
float theta;
std::cout<<"pelota "<<target<<" ro = "<<ro<<std::endl;
if(ro < 0.7){
w = v = 0.0;
sound.value=kobuki_msgs::Sound::CLEANINGSTART;
sonido.publish(sound); //AQUI SE REPRODUCE UN SONIDO
state = GOTOBIN;
cmd.linear.x = v;
cmd.angular.z = w;
}else{
theta = normalizePi(atan2(BL2B.getOrigin().y(), BL2B.getOrigin().x()));
std::cerr<<"theta: "<<fabs(theta)<<std::endl;
usoPid=velDeGiro.OperarMiPid(theta);
cmd.angular.z = usoPid;
if(fabs(theta) > 0.1){
v = 0.1;
cmd.linear.x = v;
}else{
v = 0.2;//AQUI HABRIA QUE HACER UN VFF SENCILLO QUE SOLO TENGA EN CUENTA LA DISTANCIA HASTA LA PELOTA
cmd.linear.x = v;
}
}
break;
}
case GOTOBIN:
{
if(lost){
std::cout<<"LAST POSE: "<<lastPose.pose.position.z<<std::endl;
if(lastPose.pose.position.z == -1.0){
if(count % 200 < 160){
std::cout<<"giro para encontrarme"<<std::endl;
cmd.angular.z = 0.2;
cmd.linear.x = 0.0;
}else{
std::cout<<"Avanzo para encontrarme"<<std::endl;
cmd.linear.x = 0.1;
cmd.angular.z = 0.0;
}
}else{
tf::StampedTransform W2BIn;
try{
tfL.lookupTransform("world", "bin",
ros::Time::now() - ros::Duration(1.0), W2BIn);
}catch(tf::TransformException & ex) {
ROS_WARN("%s", ex.what());
break;
}
double roll, pitch, yaw;
tf::Quaternion q(pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
angle2goal2 = normalizePi(atan2(W2BIn.getOrigin().y() - lastPose.pose.position.y,
W2BIn.getOrigin().x() -lastPose.pose.position.x) - yaw);
ETA = ros::Time::now() + ros::Duration(fabs(angle2goal2*23)/(2*M_PI));
state = GIRANDO;
diffpose2 = sqrt((lastPose.pose.position.x-W2BIn.getOrigin().x())*(lastPose.pose.position.x-W2BIn.getOrigin().x())
+ (lastPose.pose.position.y-W2BIn.getOrigin().y())*(lastPose.pose.position.y-W2BIn.getOrigin().y()));
}
break;
}else{
tf::StampedTransform W2BIn;
try{
tfL.lookupTransform("world", "bin",
ros::Time::now() - ros::Duration(1.0), W2BIn);
}catch(tf::TransformException & ex) {
ROS_WARN("%s", ex.what());
break;
}
float diffpose;
lastPose = pose;
diffpose = sqrt((pose.pose.position.x-W2BIn.getOrigin().x())*(pose.pose.position.x-W2BIn.getOrigin().x())
+ (pose.pose.position.y-W2BIn.getOrigin().y())*(pose.pose.position.y-W2BIn.getOrigin().y()));
double roll, pitch, yaw;
tf::Quaternion q(pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
float v,w, usoPid;
Pid velDeGiro;
if(diffpose < 0.1){
//std::cerr<<"diffpose: "<<diffpose<<std::endl;
w = v = 0.0;
sound.value=kobuki_msgs::Sound::CLEANINGSTART;
sonido.publish(sound);
//AQUI SE REPRODUCE UN SONIDO Y SE ELIMINA LA PELOTA
blacklist[bolas] = target;
bolas++;
state = SEARCH;
cmd.angular.z = 0.0;
cmd.linear.x = 0.0;
lastPose.pose.position.z = -1;
}else{
angle2goal = normalizePi(atan2(W2BIn.getOrigin().y() - pose.pose.position.y,
W2BIn.getOrigin().x() -pose.pose.position.x) - yaw);
//std::cerr<<"angle2goal: "<<fabs(angle2goal)<<std::endl;
usoPid=velDeGiro.OperarMiPid(angle2goal);
cmd.angular.z = usoPid;
if(fabs(angle2goal) > 0.2){
v = 0.0;
cmd.linear.x = v;
}else{
v = 0.3;//AQUI HABRIA QUE HACER UN VFF SENCILLO QUE SOLO TENGA EN CUENTA LA DISTANCIA HASTA BIn
cmd.linear.x = v;
}
}
break; //si no funciona según lo esperado, este break lo tenía puesto antes encima del case GIRANDO, pero creo que aqui
//hace mejor su función
}
}
case GIRANDO:
{
cmd.angular.z = 0.0;
cmd.angular.y = 0.0;
cmd.angular.x = 0.0;
cmd.linear.x = 0.0;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
if(ros::Time::now() < ETA){
if(angle2goal2>=0){
std::cout<<"entro a girar izquierda"<<std::endl;
cmd.angular.z = 0.3;
}else{
std::cout<<"entro a girar derecha"<<std::endl;
cmd.angular.z = -0.3;
}
}else{
state = AVANZAR_ESPARTANOS;
ETA2 = ros::Time::now() + ros::Duration(diffpose2/VELOCIDAD_LINEAL);
}
cmdpub.publish(cmd);
break;
}
case AVANZAR_ESPARTANOS:
{
cmd.angular.z = 0.0;
cmd.angular.y = 0.0;
cmd.angular.x = 0.0;
cmd.linear.x = 0.0;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
if(ros::Time::now() < ETA2){
std::cout<<"entro a avanzar"<<std::endl;
cmd.linear.x = 0.3;
}else{
sound.value=6;//kobuki_msgs::Sound::CLEANINGSTART;
sonido.publish(sound);
blacklist[bolas] = target;
bolas++;
state = VE_HACIA_ATRAS;
//voy a retroceder el mismo tiempo y a la misma velocidad de lo que habia avanzado
Tiempo_Hacia_Atras = ros::Time::now() + ros::Duration(diffpose2/VELOCIDAD_LINEAL);
}
//lastPose.pose.position.z = -1;
cmdpub.publish(cmd);
break;
}
case VE_HACIA_ATRAS:
{
cmd.angular.z = 0.0;
cmd.angular.y = 0.0;
cmd.angular.x = 0.0;
cmd.linear.x = 0.0;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
if(ros::Time::now() < Tiempo_Hacia_Atras){
std::cout<<"voy a dar marcha atras"<<std::endl;
cmd.linear.x = -0.3;
}else{
state = SEARCH;
}
lastPose.pose.position.z = -1;
cmdpub.publish(cmd);
break;
}
default:
std::cout<<"WHAT THE F**K"<<std::endl;
}
cmdpub.publish(cmd);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
/* Dict::createDict(): creates a new dictionary file using the SongDB.
*/
void Dict::createDict()
{
ofstream dictfile;
try
{
dictfile.open(dictfileName.c_str(), fstream::out | fstream::trunc);
}
catch (ifstream::failure e)
{
cout << "could not open dictionary file for output" << endl;
return;
}
SongDB::Iterator dbi = songDB.begin();
Song theSong;
string theWord;
stringstream theLyrics;
int theID;
unsigned int theHash;
int collisions = 0, inserts=0;
MapValue mv;
string t;
srand(time(NULL));
seed = rand() % 40000000;
bool flag = false;
while (dbi != songDB.end())
{
theSong = dbi.value();
theID = dbi.songId();
theLyrics << "";
theLyrics.clear();
t = theSong.getLyrics();
theLyrics.str(t);
theWord = "";
while (theLyrics >> theWord)
{
theWord = strip(theWord);
if (theWord.length() < 2)
continue;
transform(theWord.begin(), theWord.end(), theWord.begin(), ToLower());
if (USE_BLACKLIST && isBlacklisted(theWord))
continue;
theHash = MurmurHash2A(theWord.c_str(), theWord.length(), seed);
if (theMap.find(theHash) == theMap.end())
{
mv = MapValue();
//cout << " (new)";
mv.songlist = vector<SongId>();
mv.songlist.push_back(theID);
mv.key = theWord;
theMap.insert(pair<unsigned int, MapValue >(theHash,mv));
inserts++;
}
else // append
{
// check for collision
mv = theMap[theHash];
if (mv.key.compare(theWord)==0) // no collision, append
{
// cout << "No collision, append" << endl;
// cout << "songlist size before = " << mv.songlist.size() << endl;
// cout << "adding " << theID << " to the songID list for " << theWord << endl;
// check to see if the ID we're on is already on this list
flag = false;
for (unsigned int j=0; j<mv.songlist.size(); j++)
{
if (mv.songlist[j] == theID)
flag = true;
}
if (!flag)
mv.songlist.push_back(theID);
theMap[theHash] = mv;
inserts++;
}
else
{
// collision, rehash with a different seed to try again
//cout << " ** Hash Collision! " << theHash << " current: " << mv.key << " mew: " << theWord << endl;
collisions++;
// linear forward search
do
{
mv = theMap[++theHash];
}
while ((mv.songlist.size() > 0) && (mv.key.compare(theWord)!=0));
//Now, we either have the first blank address (for a new collision) or the previously resolved collision address.
if (mv.songlist.size() > 0)
{
mv = MapValue();
mv.songlist = vector<SongId>();
mv.songlist.push_back(theID);
mv.key = theWord;
inserts++;
theMap[theHash] = mv;
//cout << "Previously resolved. New address is " << theHash << endl;
}
else
{
// mv = theMap[theHash];
// cout << "Resolution: " << theHash << endl;
// cout << "Resolution: " << theHash << endl;
mv.key = theWord;
mv.songlist = vector<SongId>();
mv.songlist.push_back(theID);
theMap[theHash] = mv;
inserts++;
}
//goto restartRound; // I know.
}
}
}
if (inserts > MAX_SONGS)
break;
//return;
dbi = dbi.next();
}
cout << collisions << " collisions and " << inserts << " records (" << (((1.0*collisions)/inserts)*100) << "%)" << endl;
//Now, output the semi-ordered map to file
// cout << "outputting file..." << endl;
dictfile.put((char)0);
dictfile.write((char *)&seed, sizeof(unsigned int));
unsigned short tui = 0;
char* tc;
//cout << "Seed = " << hex << seed << endl;
dictfile.put('\n');
// dictfile << "Seed " << seed << endl;
for (map<unsigned int, MapValue>::iterator iter=theMap.begin(); iter != theMap.end(); iter++)
{
mv = (*iter).second;
// dictfile << (*iter).first << " " << mv.key << " " << mv.songlist.size() << " ";
dictfile.write((char *)&(*iter).first, sizeof(unsigned int));
//cout << hex << (*iter).first << " " << mv.key.length() << endl;
tui = mv.key.length();
dictfile.write((char *)&tui, sizeof(unsigned short));
tc = new char[tui+1];
strcpy(tc, mv.key.c_str());
// cout << "tui = " << tui << endl;
for (unsigned short j=0; j<=tui; j++)
{
// cout << tc[j];
dictfile.write(&tc[j], sizeof(char));
}
delete[] tc;
// cout << mv.key << " " << mv.songlist.size() << endl;
tui = mv.songlist.size();
dictfile.write((char *)&tui, sizeof(unsigned short));
for (unsigned short j=0; j<tui; j++)
{
dictfile.write((char*)&mv.songlist[j], sizeof(unsigned int));
}
//dictfile.put('\n');
}
dictfile.close();
}
/* Dict::search: returns a vector of Song objects matching the given search terms (up to limit results) */
list<SongResult> Dict::search(string key, unsigned int limit)
{
list<SongResult> retval;
SongResult tsr;
key = strip(key);
if (USE_BLACKLIST && isBlacklisted(key))
{
cout << key << " is on the blacklist. Exiting." << endl;
return retval;
}
unsigned int theHash = MurmurHash2A(key.c_str(), key.length(), seed);
MapValue mv = theMap[theHash];
//cout << key << " hashes to " << theHash << " and the key at that location is " << mv.key << endl;
if (mv.key.length() > 0 && mv.key.compare(key)==0) // match
{
//cout << "match found. " << mv.songlist.size() << " songs are attached." << endl;
for (unsigned int i=0; i<mv.songlist.size(); i++)
{
if ((i+1) > limit)
break;
//cout << mv.songlist[i] << " ";
tsr = SongResult();
tsr.key = key;
tsr.keylength = key.length();
tsr.id = mv.songlist[i];
tsr.song = songDB.getSongById(mv.songlist[i]);
retval.push_back(tsr);
}
//cout << endl;
}
else if (mv.key.length() > 0) // collision
{
// linear forward search
do
{
mv = theMap[++theHash];
}
while ((mv.key.length() > 0) && (mv.key.compare(key)!=0));
//Now, we ought to
if (mv.key.length() > 0)
{
for (unsigned int i=0; i<mv.songlist.size(); i++)
{
if ((i+1) > limit)
break;
//cout << mv.songlist[i] << " ";
tsr = SongResult();
tsr.key = key;
tsr.keylength = key.length();
tsr.song = songDB.getSongById(mv.songlist[i]);
retval.push_back(tsr);
}
}
else
{
cout << "Unresolved hash collision on input. Fatal error." << endl;
}
}
else
{
cout << "not found." << endl;
// no results.
}
return retval;
}
