radiometry.cpp

Go to the documentation of this file.

#include "radiometry.hpp"

int main(int argc, char **argv) {
        
        ros::init(argc, argv, "radiometry");
        
        ros::NodeHandle private_node_handle("~");
        
        radiometryData startupData;
        
        startupData.read_addr = argv[0];
        startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-12);
        bool inputIsValid = startupData.obtainStartingData(private_node_handle);
        
        if (!inputIsValid) {
                ROS_ERROR("Configuration invalid!\n");
                return -1;
        }

        ros::NodeHandle nh;
        
        ROS_INFO("Establishing radiometry node...");
        boost::shared_ptr < radiometryNode > radiometry_node (new radiometryNode (nh, startupData));
        
        globalNodePtr = &radiometry_node;
        
        if (!(*globalNodePtr)->isNodeValid()) {
                ros::shutdown();
                return 0;
        }
        
        signal(SIGINT, mySigintHandler);
        
        if (wantsToShutdown) {
                ros::shutdown();
                return 0;
        }
        
        // Then is this the main part?
        
        while ((*globalNodePtr)->dataStillStreaming()) {
                
                ros::spinOnce();
                                
        }
        
        ros::shutdown();

        return 0;
        
}

bool radiometryNode::dataStillStreaming() {
        
        // Should basically just contain a check to make sure there hasn't been too large a delay since the last frame...
        // (except for the very first)
        
        return true;
}

void radiometryNode::prepareForTermination() {
        
        if (configData.communicateWithBlackbody) {
                closeBlackbodyComms();
        }
        
        if (ofs_thermistor_log.is_open()) ofs_thermistor_log.close();
        if (ofs_image_log.is_open()) ofs_image_log.close();
        
}

bool radiometryNode::isNodeValid() {
        return nodeStillValid;
}

void mySigintHandler(int sig)
{

        ROS_WARN("Requested shutdown... terminating feeds...");
        wantsToShutdown = true;
        
        (*globalNodePtr)->prepareForTermination();
  
}




radiometryNode::radiometryNode(ros::NodeHandle& nh, radiometryData startupData) {
        
        nodeStillValid = true;
        
        temperatureIsStable = false;
        stabilityOnceOffFlag = false;
        
        blackbodyIsSetup = false;
        imageIndex = 0;
        
        stabilityTimer = 0.0;
        elapsedTime = -1.0;
        
        sprintf(nodeName, "%s", ros::this_node::getName().substr(1).c_str());
        
        ROS_INFO("Initializing node (%s)", nodeName);
        
        // Some variables...
        
        configData = startupData;
        
        
        
        ROS_INFO("Establishing server callback...");
        f = boost::bind (&radiometryNode::serverCallback, this, _1, _2);
    server.setCallback (f);
        
        if (configData.communicateWithBlackbody) {
                if (configData.serialPollingRate > 0.1) {
                        ROS_INFO("Initializing serial timer at (%f)", 1.0 / ((double) configData.serialPollingRate));
                        serial_timer = nh.createTimer(ros::Duration(1.0 / ((double) configData.serialPollingRate)), &radiometryNode::serialCallback, this);
                } else {
                        ROS_INFO("Initializing serial timer at (%f)", DEFAULT_SERIAL_POLLING_RATE);
                        serial_timer = nh.createTimer(ros::Duration(DEFAULT_SERIAL_POLLING_RATE), &radiometryNode::serialCallback, this);
                }
                
                nodeStillValid = setupBlackbodyComms();
                
                if (nodeStillValid) {
                        nodeStillValid = setupBlackbody();
                        
                        blackbodyIsSetup = true;
                
                        // Then serial callback will just be used to verify that the blackbody temperature hasn't strayed too much
                        // Also, blackbody temp will actually be saved to file like the thermistor data
                }
                
                
        }
        
        if (!setupFilesAndDirectories()) {
                wantsToShutdown = true;
                (*globalNodePtr)->prepareForTermination();
        }
        
        thermistorLogFile = configData.outputFolder + "/" + string(subdirectoryName) + "-thermistor-log.txt";
        ofs_thermistor_log.open(thermistorLogFile.c_str());
        
        imageLogFile = configData.outputFolder + "/" + string(subdirectoryName) + "-image-log.txt";
        ofs_image_log.open(imageLogFile.c_str());
        
        serialCommsFile = configData.outputFolder + "/" + string(subdirectoryName) + "-serial-log.txt";
        //ofs_serial_log.open(serialCommsFile.c_str());
        
        // Then subscribe to topics...

        if (nodeStillValid) {
                it = new image_transport::ImageTransport(nh);
                
                if (configData.verboseMode) { ROS_INFO("Subscribing to topic: (%s)", configData.thermal_topic.c_str()); }
                camera_sub = it->subscribeCamera(configData.thermal_topic, 1, &radiometryNode::handle_camera, this);
        }       
        
}

bool radiometryNode::setupFilesAndDirectories() {
        DIR * dirp;
        struct dirent * entry;
        
        dirp = opendir(configData.outputFolder.c_str());
        
        // First make parent directory if there's nothing there
        
        if (dirp != NULL) {
                ROS_WARN("Directory (%s) already exists!", configData.outputFolder.c_str());
                // return false;
        } else {
                // Create folder if necessary
                struct stat st;
                if(stat(configData.outputFolder.c_str(),&st) != 0) {
                        char folderCommand[256];
                        sprintf(folderCommand, "mkdir -p %s", configData.outputFolder.c_str());
                        int res = system(folderCommand);
                        
                        if (res == 0) {
                                // ROS_WARN("system() call returned 0 when making main folder");
                        }
                }
                ROS_WARN("Directory (%s) created.", configData.outputFolder.c_str());
        }
        
        // Determine appropriate name for subdirectory
        // subdirectoryName
        
        char coeff;
        
        if (configData.targetTemperature < 0) {
                coeff = 'n';
        } else {
                coeff = 'p';
        }

        sprintf(subdirectoryName, "deg-%c%05.02f", coeff, abs(configData.targetTemperature));
        
        //dirp = opendir(outputFolder.c_str());
        
        // Now check to see if this specific temperature is there..
        
        dirp = opendir(configData.outputFolder.c_str());
        
        while ((entry = readdir(dirp)) != NULL) {
                if (entry->d_type == DT_DIR) { // If the entry is a regular file
                        if (strcmp(entry->d_name, subdirectoryName) == 0) {
                                ROS_ERROR("Subdirectory (%s) already exists! (This temperature has already been tested.)", subdirectoryName);
                                closedir(dirp);
                                return false;
                        }
                }
        }

        closedir(dirp);
        
        // Create folder if necessary
        string fullFolder = configData.outputFolder + "/" + subdirectoryName;
        struct stat st;
        if(stat(fullFolder.c_str(),&st) != 0) {
                char folderCommand[256];
                sprintf(folderCommand, "mkdir -p %s", fullFolder.c_str());
                
                ROS_WARN("Creating subdirectory (%s)..", folderCommand);
                int res = system(folderCommand);
                
                if (res == 0) {
                        // ROS_WARN("system() call returned 0...");
                }
        }
        
        return true;
                
}

void radiometryNode::serialCallback(const ros::TimerEvent&) {
        
        //ROS_INFO("In callback...");
        
        if (!temperatureIsStable) {
                
                if (elapsedTime < 0.0) {
                        elapsedTime = 0.0;
                } else {
                        elapsedTime = (ros::Time::now().toNSec() - previousTime.toNSec()) / 1000000000.0;
                }
        
                previousTime = ros::Time::now();
                
                readTemperature(currentTemp);
                
                double tempDiff = abs(currentTemp - configData.targetTemperature);
        
                if (tempDiff > (configData.maxStableVariation + TEMP_ERROR_HACK)) {
                        stabilityTimer = 0.0;
                        ROS_WARN("Temp diff still out of range (%1.2f > %1.2f)", tempDiff, configData.maxStableVariation);
                } else {
                        //ROS_INFO("Elapsed time: (%f)", elapsedTime);
                        stabilityTimer += elapsedTime;
                
                        
                        if (stabilityTimer > configData.minStableTime) {
                                temperatureIsStable = true;
                                stabilityOnceOffFlag = true;
                                ROS_INFO("=====================================================");
                                ROS_INFO("Blackbody source sufficiently stable for calibration!");
                                ROS_INFO("=====================================================");
                                
                                // Deactivate elapsedTime until the video streaming is ready to utilize it..
                                elapsedTime = -1.0;
                        
                        } else {
                                ROS_WARN("Elapsed time at stable temperature not yet sufficient (%f < %f)", stabilityTimer, configData.minStableTime);  
                        }
                        
                        
                        
                }
                
        } else {
                
                // If in here, should actually be performing BB calibration (well, once data is streaming...)
                
                readTemperature(currentTemp);
                
                double tempDiff = abs(currentTemp - configData.targetTemperature);
        
                if (tempDiff > (configData.maxStableVariation + TEMP_ERROR_HACK)) {
                        ROS_WARN("Temp diff currently out of range (%1.2f > %1.2f)", tempDiff, configData.maxStableVariation);
                }
                
        }
        
        
        /*
         * 
         */

}

bool radiometryNode::closeBlackbodyComms() {
        
        /* Free the memory */
    free(tab_rp_bits);
    free(tab_rp_registers);

    /* Close the connection */
    modbus_close(ctx);
    modbus_free(ctx);
        
        return true;
}

bool radiometryNode::changeSetPoint(double newVal) {
        
        // ROS_INFO("Changing set point to (%f)", newVal);
        
        int rc;
        
        int integerVal = int(abs(newVal)*100);

        if (newVal < 0) {
                integerVal = 65535 - integerVal + 1;
        }
        
        uint16_t numberOfWords = 0x1;
    uint16_t sendValue[1];
        
    sendValue[0] = integerVal;
        
    //sendValue[0] = 0x0960;    // This is hard-coded to perhaps 24 degrees? want it to depend on newVal...
        
        //freopen (serialCommsFile.c_str(), "w", stdout);
        rc = modbus_write_registers(ctx, ISOTECH_SETPOINT_ADDR, numberOfWords, sendValue);
        //fclose (stdout);
        
        if (rc != numberOfWords) {
        ROS_ERROR("changeSetPoint() Failed.");
        return false;
    }

    
    return true;
        
}

bool radiometryNode::readSetpoint(double& setPoint) {
        
        //ROS_INFO("Attempting to read current params...");
        
        int rc;
        
        uint16_t numberOfWords = 0x1;
        
        uint16_t *dest;
    dest = new uint16_t[256];
    
    //freopen (serialCommsFile.c_str(), "w", stdout);
    rc = modbus_read_registers(ctx, ISOTECH_SP_ADDR, numberOfWords, dest);
    //fclose (stdout);
    
    uint16_t x;   
        std::stringstream ss;
        
        ss << std::hex << dest[0];
        ss >> x;
                
        setPoint = float(x) / 100.0;
        
        if (setPoint > MAXIMUM_BLACKBODY_TEMPERATURE) {
                setPoint = -float(65536 - x) / 100.0;
        } 
        
    if (rc != numberOfWords) {
        ROS_ERROR("readSetpoint() Failed.");
        return false;
        }

        return true;
        
}

bool radiometryNode::readTemperature(double& actualTemp) {
        
        ROS_INFO("Attempting to read current params...");
        
        int rc;
        
        uint16_t numberOfWords = 0x1;
        
        uint16_t *dest;
    dest = new uint16_t[256];
    
    //freopen (serialCommsFile.c_str(), "w", stdout);
    rc = modbus_read_registers(ctx, ISOTECH_TEMP_ADDR, numberOfWords, dest);
    //fclose (stdout);
    
    uint16_t x;   
        std::stringstream ss; // dest[3] is the 6th byte... so want 2 & 3...
        
        ss << std::hex << dest[0];
        ss >> x;
                
        actualTemp = float(x) / 100.0;
        
        if (actualTemp > MAXIMUM_BLACKBODY_TEMPERATURE) {
                actualTemp = -float(65536 - x) / 100.0;
        } 
        
    if (rc != numberOfWords) {
        ROS_ERROR("readTemperature() Failed.");
        return false;
        }

        return true;
        
}

bool radiometryNode::setupBlackbodyComms() {
        ROS_INFO("Establishing serial connection with blackbody...");

    ctx = modbus_new_rtu(configData.portAddress.c_str(), configData.baudRate, 'N', 8, 1);
    
    if (ctx == NULL) {
        ROS_ERROR("Unable to allocate libmodbus context (%s)", configData.portAddress.c_str());
        return false;
    }
    
    modbus_set_debug(ctx, TRUE);
    
    modbus_error_recovery_mode erm = (modbus_error_recovery_mode) (MODBUS_ERROR_RECOVERY_LINK | MODBUS_ERROR_RECOVERY_PROTOCOL);
    
    modbus_set_error_recovery(ctx, erm);
    
    modbus_set_slave(ctx, 1);
    
    if (modbus_connect(ctx) == -1) {
        ROS_ERROR("Connection failed: %s", modbus_strerror(errno));
        modbus_free(ctx);
        return false;
    }

        /* Allocate and initialize the memory to store the bits */
    nb_points = (UT_BITS_NB > UT_INPUT_BITS_NB) ? UT_BITS_NB : UT_INPUT_BITS_NB;
    
    tab_rp_bits = (uint8_t *) malloc(nb_points * sizeof(uint8_t));
    memset(tab_rp_bits, 0, nb_points * sizeof(uint8_t));

    /* Allocate and initialize the memory to store the registers */
    
    nb_points = (UT_REGISTERS_NB > UT_INPUT_REGISTERS_NB) ? UT_REGISTERS_NB : UT_INPUT_REGISTERS_NB;
    tab_rp_registers = (uint16_t *) malloc(nb_points * sizeof(uint16_t));
    memset(tab_rp_registers, 0, nb_points * sizeof(uint16_t));
 
        return true;
    
}

bool radiometryNode::setupBlackbody() {
        
        if (configData.verboseMode) { ROS_INFO("Setting up blackbody..."); }
        
        // This function makes sure the blackbody converges on the correct temperature
        
        // Set temperature
        
        changeSetPoint(configData.targetTemperature);
        
        // Introduce a small pause to allow the change to take effect...
            
        ros::Duration(0.5).sleep();
            
        
        double actualTemp, setPoint;
        readTemperature(actualTemp);
        readSetpoint(setPoint);
        
        ROS_INFO("Current Actual Temp = (%f)", actualTemp);
    ROS_INFO("Current Set Point = (%f)", setPoint);     
    
    if (setPoint != configData.targetTemperature) {
                ROS_ERROR("Set Point (%6.2f) is not equal to specified target temperature (%6.2f)! (consider restarting)", setPoint, configData.targetTemperature);
                return false;
        }
        
        if (configData.verboseMode) { ROS_INFO("Blackbody setup."); }
        
        blackbodyIsSetup = true;
        
        return true;
        
}

void radiometryNode::handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
        
        //ROS_INFO("Handling camera...");
                
        
        
        cv_ptr = cv_bridge::toCvCopy(msg_ptr, "16UC1");
        
        frame = cv::Mat(cv_ptr->image);
        
        float thermistorVal;
                        
        memcpy(&thermistorVal, &info_msg->binning_x, 4);
        
        char timeString[256];
        
        sprintf(timeString,"%010d.%06d ",info_msg->header.stamp.sec, info_msg->header.stamp.nsec/1000);
        
        ofs_thermistor_log << timeString << thermistorVal << endl;
        
        if (matEquality) {
                
                matEquality = matricesAreEqual(frame, lastFrame);
                
                if (!matEquality) {             
                        
                        if (temperatureIsStable) {
                                
                                
                                previousTime = ros::Time::now();
                                
                                if (elapsedTime < 0.0) {
                                        elapsedTime = 0.0;
                                } else {
                                        elapsedTime = (ros::Time::now().toNSec() - previousTime.toNSec()) / 1000000000.0;
                                }
                                
                                ROS_WARN("Fresh frame! [%d, %f, %f] (%fs elapsed since previous)", imageIndex, currentTemp, thermistorVal, elapsedTime);
                                
                                // So now want to extract data...
                                
                                ofs_image_log << timeString << " " << imageIndex << " " << thermistorVal << " " << currentTemp << endl;
                                
                                char outputFilename[256];
                                sprintf(outputFilename, "%s/%s/frame%06d.%s", configData.outputFolder.c_str(), subdirectoryName, imageIndex, "png");
                                
                                cv::imwrite(outputFilename,cv_ptr->image);
                                
                                imageIndex++;
                                
                                
                        }
                        
                }
        } else {
                matEquality = matricesAreEqual(frame, lastFrame);
        }
        
        
        
        frame.copyTo(lastFrame);
        
        if (elapsedTime > configData.completionTimeout) {
                ROS_ERROR("Time elapsed since last fresh frame has exceeded specified limit.");
                nodeStillValid = false;
        }
        
}


radiometryData::radiometryData() {
        
        verboseMode = false;
        communicateWithBlackbody = true;
    
        
}

bool radiometryData::obtainStartingData(ros::NodeHandle& nh) {
        
        nh.param<bool>("communicateWithBlackbody", communicateWithBlackbody, true);
        //nh.param<double>("minStableTime", minStableTime, DEFAULT_MIN_STABLE_TIME);
        //nh.param<double>("maxStableVariation", maxStableVariation, DEFAULT_MAX_STABLE_VARIATION);
        
        nh.param<string>("thermal_topic", thermal_topic, "/thermalvis/streamer/image_raw");
        
        nh.param<string>("outputFolder", outputFolder, "outputFolder");
        
        nh.param<string>("portAddress", portAddress, DEFAULT_PORT_ADDRESS);
        nh.param<int>("baudRate", baudRate, DEFAULT_BAUD_RATE);
        
        if (outputFolder == "outputFolder") {
                ROS_ERROR("No output folder specified!");
                return false;
        }
        
        
        // nh.param<double>("targetTemperature", targetTemperature, 9e99);
        
        
        
        
        
        
        return true;
}

void radiometryNode::serverCallback(thermalvis::radiometryConfig &config, uint32_t level) {
        
        if (configData.verboseMode){ ROS_INFO("Processing reconfigure request..."); }
        
        configData.verboseMode = config.verboseMode;
        
        if (blackbodyIsSetup) {
                if (configData.targetTemperature != config.targetTemperature) {
                        ROS_WARN("Cannot change target temperature now - directories have already been setup. Consider restarting.");
                }
        } else {
                configData.targetTemperature = config.targetTemperature;
        }
        
        
        ROS_INFO("targetTemperature = (%f)", configData.targetTemperature);
        
        if (configData.serialPollingRate != config.serialPollingRate) {
                configData.serialPollingRate = config.serialPollingRate;
                ROS_INFO("Updating period with rate (%f)", configData.serialPollingRate);
                
                if (configData.serialPollingRate > 0.1) {
                        serial_timer.setPeriod(ros::Duration(1.0 / configData.serialPollingRate));
                } else {
                        serial_timer.setPeriod(ros::Duration(1.0 / 1.0));
                }
        }
        
        configData.maxStableVariation = config.maxStableVariation;
        configData.completionTimeout = config.completionTimeout;
        configData.minStableTime = config.minStableTime;
        
        if (configData.verboseMode) { ROS_INFO("Reconfigure request complete..."); }
}