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;
}
