bool Controller::UpdateRobotData() { if(this->nextImage.empty()) { ErrorIO("nextImage.Data is null in UpdateRobotData"); return false; } else robot.GenerateCharacterizer(this->nextImage); return true; }
bool Controller::init(string dirName) { // Initiate the ros::Callback functions and shake hands with the robot. if(!RobotInit()) { ErrorIO("RobotInit failed"); return false; } // Initiate our RobotState object with the data recieved from the robot UpdateRobotData(); if(!this->ap.GetConstants(dirName)) return false; this->ap.SetDistribution(perspectives); if(!this->ap.GenerateParticles(defaultParticleListsize)) return false; this->ap.AnalyzeList(); // For MetaData! float minx = 10000; float maxx = -10000; float miny = 10000; float maxy = -10000; for (int i = 0; i < perspectives.size(); i++) { Perspective p = perspectives[i]; minx = p.x < minx ? p.x : minx; maxx = p.x > maxx ? p.x : maxx; miny = p.y < miny ? p.y : miny; maxy = p.y > maxy ? p.y : maxy; } ofstream mdFile; mdFile.open("../src/GUI/Meta/MetaData.txt"); mdFile << minx << " " << maxx << " " << miny << " " << maxy << "\n"; mdFile.close(); ofstream File; File.open("../src/GUI/PyViewer/Perspectives.txt"); for (int i = 0; i < perspectives.size(); i++) { Perspective p = perspectives[i]; File << p.x << " " << p.y << " " << p.z << " " << p.dx << " " << p.dy << " " << p.dz << " " << i << "\n"; } File.close(); return true; }
// Parse the string that contains the 4 movement commands, [x y z dtheta] with theta being in the xy plane void Controller::MovementCallback(const std_msgs::String msg) { //DebugIO("Inside of movement callback"); std::string str = msg.data; // stringstream ss; // ss << "Recieved move command " << str; // DebugIO(ss.str()); int state; std::vector<std::string> strs; boost::split(strs, str, boost::is_any_of(" _,")); state = atoi(strs[0].c_str()); // parse token to int if(state == killflag) { EXIT_FLAG = true; return; } //std::cout << "State value from robot : " << state << std::endl; if(state == starting_move) { moving = true; // DebugIO("Starting move value recieved"); recentMove.clear(); return; } else if(state == finished_move) { // DebugIO("Finished Move Value Recieved"); moving = false; for(int i = 1; i < strs.size(); i++) { float value = atof(strs[i].c_str()); recentMove.push_back(value); } // std::cout << "Movedetected : Translation = " << recentMove[0] << " || Rotation = " << recentMove[1] << std::endl; } else { stringstream ss; ss << "Error : State command from robot must be " << starting_move << " (start move), " << finished_move << " (stop move), or " << killflag << " (killflag)"; ErrorIO(ss.str()); } }
// recieves and processes images that are published from the robot or other actor. It is up to the user to set up // their own publisher and to convert their image data to a sensor_msgs::Image::ConstPtr& . void Controller::ImageCallback(const sensor_msgs::Image::ConstPtr& img) { //DebugIO("Recieved Img from Robot"); cv_bridge::CvImagePtr im2; try { im2 = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::BGR8); } catch (...) { ErrorIO("ImageCallback toCvCopy call failed"); } if(!im2->image.empty()) { if(!image_feed_started) image_feed_started = true; this->nextImage = im2->image; } else ErrorIO("Image Data from Robot is Null"); }
// Start by sending a movement command to the robot, then update every particle in the particle list accordingly bool Controller::MoveUpdate() { recentMove.clear(); bool (Controller::*connect_flag) (); connect_flag = &Controller::robotIsMoving; //publish the ready-move command to the robot program. std_msgs::String msg; stringstream ss; ss << readymove << " "; msg.data = ss.str(); mclDataPublisher.publish(msg); //ros::spinOnce(); bool started = PauseState(connect_flag, 5); if(!started) { ErrorIO("MoveUpdate - robot did not send start_move Signal"); this->ap.Move(0, 0); return false; } else { DebugIO("Robot has Started Moving"); ros::spinOnce(); while(moving) { ros::spinOnce(); } DebugIO("Robot has finished moving"); this->ap.Move(recentMove[0] * GRIDCONVERSION, recentMove[1]); return true; } }
// Called when the robot program is bool Controller::RobotInit() { stringstream ss; bool (Controller::*connect_flag) (); // fucntion pointer that our pause function will use to tell when we have // successfully connected to the robot DebugIO("Attempting to Subscribe to Robot ros::Publishers ... "); // Advertise our Data publisher mclDataPublisher = this->rosNodePtr->advertise<std_msgs::String>(MCL_PUBLISHER_NAME, 4); connect_flag = &Controller::publisherConnected; // Wait for the connection with our Data Publisher to succeed or time out. bool connection_succeded = this->PauseState(connect_flag, 30); if(!connection_succeded) { ErrorIO("Robot Program Failed to Subscribe to mclDataPublisher after 10 seconds. (RobotInit(..))"); return false; } else DebugIO("Robot Has Subscribed to mclDataPublisher."); this->PublishData(handshake, "_"); // pause for a second to let the robot start it's publishers. bool x = false; PauseState(&x, 2); // Subscribe to the robot movement data publisher. robotMovementSubscriber = this->rosNodePtr->subscribe(this->ROBOT_MOVEMENT_PUBLISHER_NAME, 2, &Controller::MovementCallback, this); // wait max 10 seconds for us to connect to the movement data publisher. connect_flag = &Controller::movementSubscriberConnected; connection_succeded = this->PauseState(connect_flag, 10); if(!connection_succeded) { ErrorIO("Robot Movement Publisher Failed to be Detected after 10 seconds of waiting. (RobotInit(..))"); return false; } else DebugIO("Localization Program Has Subscribed to the Robot Movement Publisher."); // pause again for the robot to boot up the image callback. x = false; PauseState(&x, 2); // subscribe to the robot image publisher (implemented through the ros::ImageTransport class) image_transport::ImageTransport it(*rosNodePtr); robotImageSubscriber = it.subscribe(this->ROBOT_IMAGE_PUBLISHER_NAME, 2,&Controller::ImageCallback, this); // wait max 10 seconds for the subscriber to connect. connect_flag = &Controller::imageSubscriberConnected; connection_succeded = this->PauseState(connect_flag, 10); if(!connection_succeded) { ErrorIO("Robot Image Publisher Failed to be Detected after 10 seconds of waiting. (RobotInit(..))"); return false; } else DebugIO("Successfully Connected to robot Image Feed"); ros::spinOnce(); // wait max 10 seconds for the subscriber to connect. connect_flag = &Controller::imageFeedStarted; connection_succeded = this->PauseState(connect_flag, 20); if(!connection_succeded) { ErrorIO("RobotInit() - Waited 30 Seconds for Robot to Publish Image Data But None Has Been Recieved"); return false; } else DebugIO("Image data has been recieved"); ros::spinOnce(); return ros::ok(); }
bool Controller::SpinOnce() { time_t tstart = time(0); ros::spinOnce(); CompareFeatures(); time_t duration = time(0) - tstart; stringstream ss; ss << "[Controller] CompareFeatures took " << duration << " seconds."; DebugIO(ss.str()); tstart = time(0); ss.str(""); this->ap.AnalyzeList(); robot.SetWeightedPerspective(ap.GetGuess()); this->PublishData(robotdata, " "); duration = time(0) - tstart; ss << "[Controller] AnalyzeList took " << duration << " seconds."; DebugIO(ss.str()); tstart = time(0); ss.str(""); ros::spinOnce(); if(!GenDistributionAndSample()) { ErrorIO("[Controller] Failed to generate distribution and sample new particles"); this->EXIT_FLAG = true; return false; } duration = time(0) - tstart; ss << "[Controller] GenDistributionAndSample took " << duration << " seconds."; DebugIO(ss.str()); tstart = time(0); ss.str(""); ros::spinOnce(); if(!MoveUpdate()) { ErrorIO("[Controller] MoveUpdate Failed"); return false; } duration = time(0) - tstart; ss << "[Controller] MoveUpdate took " << duration << " seconds."; DebugIO(ss.str()); ros::spinOnce(); UpdateRobotData(); ros::spinOnce(); return true; }
std::vector<Particle> GetParticleList() { std::vector<std::vector<float> > parts; float minweight = 1000, maxweight = 0; std::vector<float> v; std::string str; particles.clear(); return std::vector<Particle>(); // --- Open the file of the particle positions -- // std::string fn = "../../PyViewer/ParticleLists.txt"; std::ifstream file(fn.c_str()); if ( !file.is_open() ) { std::stringstream ss; ss << "ParticleLists.txt not found."; ErrorIO(ss.str()); std::vector<Particle> v; return v; } // -- Go through the file and add the particles to a map --// while (!file.eof()) { getline( file, str ); std::vector<std::string> strs; boost::split(strs, str, boost::is_any_of(" ")); for (int i = 0; i < strs.size(); i++) v.push_back(atof(strs[i].c_str())); float wt = v[6]; if(v.size() == 0) continue; if(wt > maxweight) maxweight = wt; else if(wt < minweight) minweight = wt; parts.push_back(v); v.clear(); } // -- Go through std::map<std::pair<float, float>, int> Map; for(int i = 2; i < parts.size(); i++) { std::vector<float> v = parts[i]; std::pair<float, float> temppair(v[0], v[1]); if(!Map.count(temppair)) { Map[temppair] = 1; } else Map[temppair]++; int x = Map.at(temppair); MyParticle temp(v[0], v[1], v[2], v[3], v[4], v[5], v[6], x-1, 0); particles.push_back(temp); } v.clear(); v = parts[0]; MyParticle temp(v[0], v[1], v[2], v[3], v[4], v[5], v[6], 0, 2); particles.push_back(temp); v.clear(); v = parts[1]; MyParticle temp1(v[0], v[1], v[2], v[3], v[4], v[5], v[6], 0, 1); particles.push_back(temp1); MyParticle temp2(0.0, 0.0, -50.0, 0.0, 1.0, 0.0, 0, 0, 0); particles.push_back(temp2); return pList; }