int main(int argc, char **argv) {
RoboRIOConnection connection;
connection.start("10.7.51.2", "8001");
// connection.send(TotePose(TotePose::ToteEndpoint(4, 3), TotePose::ToteEndpoint(10, 3), 50));
//
// while (true) {
// connection.send(TotePose(TotePose::ToteEndpoint(4, 3), TotePose::ToteEndpoint(10, 3), 50));
// }
LidarSerialDataCollector lidarSerialDataCollector("/dev/cu.usbserial-A903IUON", 115200);
lidarSerialDataCollector.start();
LidarDataProcessor dataProcessor;
// Setup Serial Port
serial::Serial lidarSpeedController("/dev/tty.wchusbserial1410", 9600, serial::Timeout::simpleTimeout(1000));
while (lidarSerialDataCollector.isRunning()) {
lidarSpeedController.write("a200");
std::cout << lidarSpeedController.read(3) << std::endl;
if (lidarSerialDataCollector.serialPacket != NULL) {
// std::cout << ZeroPadNumber((int)lidarSerialDataCollector.serialPacket->getSpeed()) << std::endl;
lidarSpeedController.write("a" + ZeroPadNumber((int)lidarSerialDataCollector.serialPacket->getSpeed()));
std::cout << lidarSpeedController.read(3) << std::endl;
} else {
lidarSpeedController.write("a" + ZeroPadNumber((int)0));
}
connection.send(dataProcessor.processLidarData(lidarSerialDataCollector.getOutput()));
}
connection.stop();
return 0;
}
//---------------------------------------------------------------
string FileHandler_C::Get_Name(){
stringstream filename ;
filename << directory << "/"<<name ;
if(parallel) filename <<".b"<< ZeroPadNumber(4, block) ;
if(series) filename <<"." << ZeroPadNumber(4, counter) ;
filename <<"."<< appendix ;
return filename.str() ;
};
int houdini_file_saver::writeFrameToFile(particle* particles, const simulation_parameters& parameters) {
//TODO Find a way to use the same struct for the simulation and writing to file
houdini_Particle *houdini_Particles = new houdini_Particle[parameters.particles_count];
std::stringstream ss;
ss << frames_folder_prefix << OUTPUT_FILE_NAME << ZeroPadNumber(++frame_count) << ".geo";
std::string fileName = ss.str();
for(int i=0;i<parameters.particles_count;i++){
houdini_Particles[i].px = particles[i].position.s[0];
houdini_Particles[i].py = particles[i].position.s[1];
houdini_Particles[i].pz = particles[i].position.s[2];
houdini_Particles[i].vx = particles[i].velocity.s[0];
houdini_Particles[i].vy = particles[i].velocity.s[1];
houdini_Particles[i].vz = particles[i].velocity.s[2];
//TODO Use actual colors
houdini_Particles[i].colorR =
particles[i].density > 1000.f && particles[i].density <= 2000.f ?
(particles[i].density - 1000.f) / 1000.f :
0.f;
houdini_Particles[i].colorG =
particles[i].density >= 0.f && particles[i].density < 1000.f ?
1.f - (particles[i].density) / 1000.f :
0.f;
houdini_Particles[i].colorB =
particles[i].density >= 500.f && particles[i].density <= 1000.f ?
(particles[i].density - 500.f) / 500.f :
particles[i].density >= 1000.f && particles[i].density <= 1500.f ?
1.f - (particles[i].density - 1000.f) / 500.f:
0.f;
houdini_Particles[i].mass = parameters.particle_mass;
}
std::ofstream fichier(fileName, std::ios::out | std::ios::trunc);
if (fichier.is_open())
{
ConcreteDataProvider dataProvider(houdini_Particles, parameters.particles_count);
HoudiniFileDumpParticles partsDumper(&dataProvider);
partsDumper.dump(fichier);
fichier.close();
}
else
{
std::cerr << "Error while writing to " << fileName << std::endl;
}
delete[] houdini_Particles;
return 0;
}
void simulateGeometry(size_t frameRate,
double minRadius, // in mm
double maxRadius, // in mm
double simDuration, // in seconds
size_t numBubbles, // over lifetime of simulation
const std::string &outputDir) {
std::vector<FakeBubbleStats> bubbleStats(numBubbles);
std::vector<Bubble *> bubbles(numBubbles);
size_t numFrames = ceil(frameRate * simDuration);
double dt = 1.0 / double(frameRate);
// initialize random bubble stats
for (size_t i = 0; i < numBubbles; i++) {
bubbleStats[i].bubbleBirthtime = random_double(0.02, simDuration * 0.95);
bubbleStats[i].bubbleRadius = random_double(minRadius, maxRadius);
std::cout << "bubbleRadius: " << bubbleStats[i].bubbleRadius << std::endl;
bubbleStats[i].isBubbleBorn = false;
bubbleStats[i].isBubbleDead = false;
}
// initialize bubble objects
for (size_t i = 0; i < numBubbles; i++) {
bubbles[i] = new Bubble();
TriangleMesh *currBubble = new TriangleMesh;
// yep, for now a different copy of each
currBubble->read(baseDir + "bubble_lr.obj", MFF_OBJ);
// constant is here because starting size of bubbles is 5mm radius
double ds = bubbleStats[i].bubbleRadius / 5.0;
currBubble->scale(Vector3d(ds, ds, ds));
double dx = random_double(-0.05, +0.05);
double dz = random_double(-0.05, +0.05);
Vector3d dpos(dx,0,dz);
//currBubble->translate(dpos); // TODO: uncomment this line to randomizing starting position
bubbles[i]->setBubbleMesh(currBubble);
}
std::string solidPrefix = "solid_";
std::string airPrefix = "air_";
std::string bubblePrefix = "bubble_";
for (size_t f = 0; f < numFrames; f++) {
double currTime = f * dt;
std::string zeropadFrameNum = ZeroPadNumber(f, 6);
// add a bit of random vertical motion to the fluid surface
//airMesh->jitter(Vector3d(0,0.0005,0));
solidMesh->write(outputDir + solidPrefix + zeropadFrameNum + ".obj", MFF_OBJ);
airMesh->write(outputDir + airPrefix + zeropadFrameNum + ".obj", MFF_OBJ);
// advect all the bubbles for each timestep
for (size_t b = 0; b < numBubbles; b++) {
if (bubbleStats[b].bubbleBirthtime <= currTime) {
bubbleStats[b].isBubbleBorn = true;
}
// check whether bubble has peeked above the surface
if (bubbleStats[b].isBubbleBorn && !bubbleStats[b].isBubbleDead) {
BoundingBox airbbox, bubbbox;
airMesh->getBoundingBox(airbbox);
bubbles[b]->getBubbleMesh()->getBoundingBox(bubbbox);
// rough crappy rule of thumb
if (bubbbox.GetBoxmax().y() > airbbox.GetBoxmax().y()) {
// KILL THE BUBBLE!
bubbleStats[b].isBubbleDead = true;
}
}
if (bubbleStats[b].isBubbleBorn && !bubbleStats[b].isBubbleDead) {
std::string zeropadBubbleNum = ZeroPadNumber(b, 6);
bubbles[b]->getBubbleMesh()->write(outputDir + bubblePrefix + zeropadBubbleNum + "_" + zeropadFrameNum + ".obj", MFF_OBJ);
bubbles[b]->timestep(dt);
}
}
}
}
