run_save_mod.cpp

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API

#include <fstream>              // File IO
#include <iostream>             // Terminal IO
#include <sstream>              // Stringstreams
using namespace std;
#include <limits>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <iomanip>
#include <thread>
#include <mutex>
#include <memory>
#include <functional>
#include <thread>
#include <string.h>
#include <chrono>
#include <stdexcept>
#include <ctime>

using namespace std::chrono;

//Saving firmware for FishSense prototype 2.0 (TX2)
//February 12th, 2022
//Peter Tueller ptueller@eng.ucsd.edu

std::string exec(const char* cmd) {
    char buffer[128];
    std::string result = "";
    FILE* pipe = popen(cmd,"r");
    if(!pipe) throw std::runtime_error("popen() failed!");
    try {
        while(fgets(buffer, sizeof buffer, pipe) != NULL) {
            result += buffer;
        }
    } catch(...) {
        pclose(pipe);
        throw;
    }
    pclose(pipe);
    return result;
}

int main(int argc, char* argv[]) try
{
    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_DEPTH,1280,720,RS2_FORMAT_Z16,15);
    cfg.enable_stream(RS2_STREAM_COLOR,1280,720,RS2_FORMAT_RGB8,15);

    int run_index = 0;
    fstream runfile;
    runfile.open("/usr/local/share/FishSense/run.txt");
    runfile>>run_index;
    runfile.close();
    run_index++;
    std::stringstream dirprefix;
    dirprefix << "/mnt/data/" << run_index;
    std::ofstream ofs;
    ofs.open("/usr/local/share/FishSense/run.txt", std::ofstream::out | std::ofstream::trunc);
    ofs << run_index;
    ofs.close();
    int check;
    const std::string tmp = dirprefix.str();
    check = mkdir(tmp.c_str(),0777);
    if(check) {
        //exit(1);
    }

    int bag_index = 0;
    bool rec_flag = false;

    std::stringstream bag_location;
    bag_location << dirprefix.str().c_str() << bag_index << ".bag";
    cfg.enable_record_to_file(bag_location.str().c_str());

    //Set up GPIOs
    system("echo 388 > /sys/class/gpio/export");
    cout << "388 export\n";
    system("echo 298 > /sys/class/gpio/export");
    cout << "298 export\n";
    system("echo in > /sys/class/gpio/gpio388/direction");
    cout << "388 in\n";
    system("echo out > /sys/class/gpio/gpio298/direction");
    cout << "298 out\n";
    system("echo 1 > /sys/class/gpio/gpio298/active_low");
    cout << "298 active low\n";
    
    // MO: add another gpio out here
    system("echo 480 > /sys/class/gpio/export");
    cout << "NEW 480 export\n";
    system("echo out > /sys/class/gpio/gpio480/direction");
    cout << "NEW 480 out\n";
    system("echo 1 > /sys/class/gpio/gpio480/active_high");
    cout << "NEW 480 active HIGH\n";
   

    //Blink the REC LED to prove that the system has booted and is running the program
    for(int i=0; i<5; i++) {
        system("echo 0 > /sys/class/gpio/gpio298/value");
        std::this_thread::sleep_for(std::chrono::milliseconds(250));
        system("echo 1 > /sys/class/gpio/gpio298/value");
        std::this_thread::sleep_for(std::chrono::milliseconds(250));
        cout << "blink\n";
    }

    while(true) {
        // MO: remove the below if statement so that we start recording right away without the reed
      //  if() { // goes in here no matter what to start recording 
            cout << "REMOVED reed switch\n";
            rs2::pipeline pipe;
            if(!rec_flag) { //And are not already recording
		// blink LED when recording starts 
                system("echo 0 > /sys/class/gpio/gpio298/value"); //Turn on REC LED
		std::this_thread::sleep_for(std::chrono::milliseconds(250));
        	system("echo 1 > /sys/class/gpio/gpio298/value");
        	std::this_thread::sleep_for(std::chrono::milliseconds(250));
                rec_flag=true;
                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                pipe.start(cfg); //bag file should be saving at this point!
		        cout << "saved bag to " << bag_location.str() << endl;
           // }

            auto start = high_resolution_clock::now();
            auto duration = 100;

            auto now = high_resolution_clock::now();
            auto runtime = duration_cast<microseconds>(now - start);
		pipe.stop();
		system("echo 0 > /sys/class/gpio/gpio298/value"); //Turn on REC LED
            while(rec_flag && (runtime.count() < duration)) {
		cout << "in inner loop\n";
                // MO: check time stamp
                // set rec_flag to false after 30 sectons using the time stamp
                // if you keep it in this loop, you're sure to save each bag without interrupting in between

                //We are having fun and saving the bag, all is well in the world
                //Fun is continuing to be had
               
                    //system("echo 1 > /sys/class/gpio/gpio298/value"); //Turn off REC LED
                    //pipe.stop(); //This should save the bag file
                    //system("echo 0 > /sys/class/gpio/gpio298/value"); //Turn off REC LED
                    system("sync");
                    system("sync");
                    system("sync");
                    //system("echo 1 > /sys/class/gpio/gpio298/value"); //Turn off REC LED
                    //system("umount /mnt/data");
                    //system("echo 0 > /sys/class/gpio/gpio298/value"); //Turn off REC LED
                    //system("mount /mnt/data");
                    //system("echo 1 > /sys/class/gpio/gpio298/value"); //Turn off REC LED
                    //Prep the potential next bag file
                    bag_index++;
                    std::stringstream bag_location_new;
                    bag_location_new << dirprefix.str().c_str() << bag_index << ".bag";
                    cfg.enable_record_to_file(bag_location_new.str().c_str());
                    std::this_thread::sleep_for(std::chrono::milliseconds(2000)); //Don't want to catch multiple magnetic switches
		now = high_resolution_clock::now();
                runtime = duration_cast<microseconds>(now - start);
                }

            }

            // MO: write to 1 to new configured GPIO out order to signal to the STM who will read as gpio IN
           // system("echo 1 > /sys/class/gpio/gpio480/value"); //Turn on gpio LED
	    system("echo 1 > /sys/class/gpio/gpio298/value"); //Turn on REC LED
            cout << "wrote 1 to turn off, active low to gpio, stopped recording\n";
        //}
        
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    //We should never reach here
    system("reboot");
    return EXIT_SUCCESS;
}
catch (const rs2::error& e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}