$search

streamer.cpp

Go to the documentation of this file.

#include "streamer.hpp"

#include "cxcore.h"
#include "highgui.h"

int main(int argc, char **argv) {
        
        ros::init(argc, argv, "streamer");
        
        ros::NodeHandle private_node_handle("~");
        
        streamerData 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 streamer node...");
        boost::shared_ptr < streamerNode > streamer_node (new streamerNode (nh, startupData));
        
        globalNodePtr = &streamer_node;
        
        signal(SIGINT, mySigintHandler);
        
        //streamerNode streamer_node(nh, startupData);
        
        if (wantsToShutdown) {
                ros::shutdown();

                return 0;
        }
        
        
        if ((startupData.subscribeMode) || (startupData.resampleMode)) {
                streamer_node->runBag();
        }
        
        if (startupData.readMode) {
                streamer_node->runRead();
        }
        
        if (startupData.loadMode) {
                streamer_node->runLoad();
        }
        
        if ((startupData.captureMode) || (startupData.pollMode)) {      
                streamer_node->runDevice();
        }
        
        ros::shutdown();

        return 0;
        
}

void streamerNode::prepareForTermination() {
        if (ofs.is_open()) {
                ofs.close();
        }
        
        if (ofs_call_log.is_open()) {
                ofs_call_log.close();
        }
        
        if (ofs_retrieve_log.is_open()) {
                ofs_retrieve_log.close();
        }
        
        if (ofs_internal_log.is_open()) {
                ofs_internal_log.close();
        }
        
        if (ofs_write_log.is_open()) {
                ofs_write_log.close();
        }
        
        if (ofs_duplicates_log.is_open()) ofs_duplicates_log.close();
        
        if (ofs_thermistor_log.is_open()) ofs_thermistor_log.close();
        
        if (configData.serialComms) {
                close(mainfd);
        }
        
}

void mySigintHandler(int sig)
{
        // Do some custom action.
        // For example, publish a stop message to some other nodes.

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

        

        //shutdown();

        // All the default sigint handler does is call shutdown()
  
}

bool streamerNode::setupDevice() {
        
        if (configData.verboseMode) { ROS_INFO("Entered setupDevice()..."); }
        
        colourMat.release();
        
        if ((configData.inputDatatype == DATATYPE_8BIT) || (configData.inputDatatype == DATATYPE_MM)) {
                
                ROS_INFO("Setting up device in 8-bit/MM mode...");
                getVideoCapture()->open(device_num);
        } else if (configData.inputDatatype == DATATYPE_RAW) {
                ROS_INFO("Setting up device in 16-bit mode...");
                
                int deviceWidth, deviceHeight;
                getMainVideoSource()->setup_video_capture(configData.capture_device.c_str(), deviceWidth, deviceHeight, configData.verboseMode);
                
                // Now use this opportunity to test/correct?
                
                if ((!configData.imageDimensionsSpecified) && (!configData.intrinsicsProvided)) {
                        // Use these values..
                        ROS_INFO("Using image size learned from device initialization (%d, %d)", deviceWidth, deviceHeight);
                        globalCameraInfo.imageSize.at<unsigned short>(0, 0) = deviceWidth;
                        globalCameraInfo.imageSize.at<unsigned short>(0, 1) = deviceHeight;
                        globalCameraInfo.cameraSize = Size(globalCameraInfo.imageSize.at<unsigned short>(0, 0), globalCameraInfo.imageSize.at<unsigned short>(0, 1));
        
                        overwriteCameraDims();
        
                } else {
                        // Compare the values to what you are using, and produce a warning if necessary...
                        
                        if ((globalCameraInfo.cameraSize.width != deviceWidth) || (globalCameraInfo.cameraSize.height != deviceHeight)) {
                                ROS_WARN("Device-based image size estimates (%d, %d) conflict with provided image size (%d, %d)", deviceWidth, deviceHeight, globalCameraInfo.cameraSize.width, globalCameraInfo.cameraSize.height);
                        }
                        
                }
                
        }
        
        ROS_INFO("Device set up!");
        
        setValidity(true);
        
        return true;
}

void streamerNode::releaseDevice() {
        
        if ((isVideoValid()) && (configData.verboseMode)) { ROS_WARN("setValidity(false) : releaseDevice()"); }
        setValidity(false);
        
        if ((configData.inputDatatype == DATATYPE_8BIT) || (configData.inputDatatype == DATATYPE_MM)) {
                getVideoCapture()->release();
        } else if (configData.inputDatatype == DATATYPE_RAW) {
                
                getMainVideoSource()->close_video_capture();
                
        }
}


bool streamerNode::runDevice() {
        
        device_num = atoi(&(configData.capture_device.c_str(), configData.capture_device.at(configData.capture_device.length()-1)));
        
        if (configData.captureMode) {
                ROS_INFO("Video stream (device: %d) started...", device_num);
        } else if (configData.pollMode) {
                ROS_INFO("Video polling (device: %d) started...", device_num);
        }
        
        setupDevice();
        
        while (isVideoValid()) {
                if (configData.verboseMode){ ROS_INFO("Starting loop.."); }
                if (configData.captureMode) {
                        if (configData.verboseMode){ ROS_INFO("About to spin"); }
                        ros::spinOnce();
                        if (configData.verboseMode){ ROS_INFO("Spun"); }
                        if (streamCallback()) {
                                
                                if (configData.verboseMode){ ROS_INFO("About to process"); }
                                if (processImage()) {
                                        
                                        if (configData.verboseMode){ ROS_INFO("Processed image."); }
                                        
                                        if (readyToPublish) {
                                                if (configData.verboseMode){ ROS_INFO("About to publish topics..."); }
                                                publishTopics();                
                                                if (configData.verboseMode){ ROS_INFO("Topics published."); }
                                                writeData();
                                                if (configData.verboseMode){ ROS_INFO("Data written."); }
                                        }
                                }
                        }
                        
                } else if (configData.pollMode) {
                        
                        // Want to keep on calling streamCallback until the time is right to capture the frame
                        streamCallback(false);
                        ros::spinOnce();
                        
                }
                if (configData.verboseMode){ ROS_INFO("Ending loop."); };
        }
        
        ROS_WARN("About to release device here...");
        releaseDevice();
        
        if (configData.captureMode) {
                ROS_INFO("Video capture terminating...");
        } else if (configData.pollMode) {
                ROS_INFO("Video polling terminating...");
        }
        
        return true;
}

bool streamerNode::runLoad() {
        ROS_INFO("Image load started...");
        
        if (!processFolder()) {
                ROS_ERROR("Processing of folder failed...");
                return false;
        }
        
        do {
                
                frameCounter = 0;
                setValidity(true);
                
                while (isVideoValid()) {
                        ros::spinOnce();                        
                }
                
        } while (configData.loopFlag && !wantsToShutdown);
        
        return true;
        
}

bool streamerNode::runRead() {
        
        ROS_INFO("Video reading started...");
                
        do {
                
                setValidity(true);
                
                ROS_WARN("Opening file (%s)...", configData.filename.c_str());
                if (configData.inputDatatype == DATATYPE_RAW) {
                        if (getMainVideoSource()->setup_video_file(configData.filename) < 0) {
                                ROS_ERROR("Source configuration failed.");
                        }
                } else if ((configData.inputDatatype == DATATYPE_8BIT) || (configData.inputDatatype == DATATYPE_MM)) {
                        getVideoCapture()->open(configData.filename.c_str());
                        
                        if(!cap.isOpened()) { // check if we succeeded
                                ROS_ERROR("File open failed (using OpenCV).");
                        }
                        //capture = cvCaptureFromAVI(configData.filename.c_str());
                }       
                
                ROS_WARN("Source configured.");
                
                while (isVideoValid()) {
                        ros::spinOnce();
                }
                
                ROS_WARN("Video complete.");
                
                if (configData.inputDatatype == DATATYPE_RAW) {
                        getMainVideoSource()->close_video_file(configData.filename);
                } else if ((configData.inputDatatype == DATATYPE_8BIT) || (configData.inputDatatype == DATATYPE_MM)) {
                        getVideoCapture()->release();
                }

        } while (configData.loopFlag && !wantsToShutdown);
        
        ROS_INFO("Video reading terminating...");
        
        return true;
        
}

bool streamerNode::runBag() {
        
        if (configData.subscribeMode) {
                ROS_INFO("Subscription started...");
        } else if (configData.resampleMode) {
                ROS_INFO("Resampling subscription started...");
        }

        while (isVideoValid()) {
                
                ros::spinOnce();
                                
        }
        
        if (configData.subscribeMode) {
                ROS_INFO("Subscription terminating...");
        } else if (configData.resampleMode) {
                ROS_INFO("Resampling subscription terminating...");
        }
        
        return true;
}


bool streamerNode::processFolder() {
        
        DIR * dirp;
        struct dirent * entry;
        
        dirp = opendir(configData.folder.c_str());
        
        if (dirp == NULL) {
                ROS_ERROR("Opening of directory (%s) failed.", configData.folder.c_str());
                return false;
        }

        while ((entry = readdir(dirp)) != NULL) {
                if (entry->d_type == DT_REG) { // If the entry is a regular file
                        inputList.push_back(string(entry->d_name));
                        fileCount++;
                }
        }

        closedir(dirp);

        sort(inputList.begin(), inputList.end());

        if(fileCount == -1)     {
                ROS_ERROR("File counting error.\n");
                return false;
        }

        ROS_INFO("No. of images in folder = %d\n", fileCount);

        if (fileCount == 0) {
                ROS_ERROR("Returning, because no images are in folder.\n");
                return false;
        }
        
        return true;
}

void streamerNode::markCurrentFrameAsDuplicate() {
        
        //camera_info.binning_x = 1; // this one is used for thermistor
        camera_info.binning_y = 1; // this one is to be used for duplicates
}

void streamerNode::updateCameraInfo() {
        
        
        int frameIndex;
        ros::Time currTime;
        
        unsigned int bx, by;
        
        if (configData.loadMode) {
                // TODO: loadMode: use image timestamp
                frameIndex = frameCounter-1;
                currTime = ros::Time::now();
                bx = 0;
                by = 0;
        } else if (configData.readMode) {
                // TODO: readMode: use video timestamp
                frameIndex = frameCounter-1;
                currTime = ros::Time::now();
                bx = 0;
                by = 0;
        } else if (configData.resampleMode) {
                frameIndex = frameCounter-1;
                currTime = original_time;
                bx = original_bx;
                by = original_by;
        } else if (configData.subscribeMode) {
                frameIndex = frameCounter-1;
                currTime = original_time;
                bx = original_bx;
                by = original_by;
        } else if (configData.captureMode) {
                frameIndex = frameCounter-1;
                currTime = ros::Time::now();
                bx = (unsigned int) (internal_time & 0xffffffffUL);
                by = (unsigned int) (internal_time >> 32);
        } else if (configData.pollMode) {
                frameIndex = frameCounter-1;
                currTime = ros::Time::now();
                bx = (unsigned int) (internal_time & 0xffffffffUL);
                by = (unsigned int) (internal_time >> 32);
        }
        
        camera_info.header.stamp = currTime;
        msg_color.header.stamp = currTime;
        msg_16bit.header.stamp = currTime;
        msg_8bit.header.stamp = currTime;
        
        // Use thermistor value:
        
        if (configData.readThermistor) {
                memcpy(&camera_info.binning_x, &lastThermistorReading, sizeof(float));
        }
        
        memcpy(&camera_info.R[0], &firmwareTime, sizeof(float));
        
        camera_info.binning_y = 0;

}

void streamerNode::assignCameraInfo() {
        
        ROS_INFO("Entered assignCameraInfo()...");
        
        char frame_id[256];
        sprintf(frame_id, "%s_%s_%s", "thermalvis", nodeName, "optical_frame");
        camera_info.header.frame_id = string(frame_id);
        msg_color.header.frame_id = string(frame_id);
        msg_16bit.header.frame_id = string(frame_id);
        msg_8bit.header.frame_id = string(frame_id);
        
        camera_info.height = globalCameraInfo.imageSize.at<unsigned short>(0, 1); // DEFAULT_IMAGE_HEIGHT
        camera_info.width = globalCameraInfo.imageSize.at<unsigned short>(0, 0); // DEFAULT_IMAGE_WIDTH


        /* use data from extrinsics file
        if (globalExtrinsicsData.distCoeffs0.cols != 5) { //*/

        // /* use data from intrinsics file
        if (globalCameraInfo.distCoeffs.cols != 5) { //*/

                camera_info.distortion_model = "rational_polynomial";
                ROS_INFO("Camera model : RATIONAL POLYNOMIAL - (%d) coeffs", globalCameraInfo.distCoeffs.cols);
        } else {
                camera_info.distortion_model = "plumb_bob";
                ROS_INFO("Camera model : PLUMB BOB - (%d) coeffs", globalCameraInfo.distCoeffs.cols);
        }

        camera_info.D.clear();


        // /* use data from intrinsics file
        for (int iii = 0; iii < globalCameraInfo.distCoeffs.cols; iii++) { //orig
                camera_info.D.push_back(globalCameraInfo.distCoeffs.at<double>(0, iii)); //orig
        }

        /* use data from extrinsics file
        for (int iii = 0; iii < globalExtrinsicsData.distCoeffs0.cols; iii++) {
            if (configData.camera_number==0){
                camera_info.D.push_back(globalExtrinsicsData.distCoeffs0.at<double>(0, iii));
            }else if(configData.camera_number==1){
                camera_info.D.push_back(globalExtrinsicsData.distCoeffs1.at<double>(0, iii));
            }
        }
        //*/


        if (globalCameraInfo.cameraMatrix.rows == 3) {
                for (unsigned int iii = 0; iii < 3; iii++) {
                        for (unsigned int jjj = 0; jjj < 3; jjj++) {
                                camera_info.K[iii*3 + jjj] = globalCameraInfo.cameraMatrix.at<double>(iii,jjj); //orig
                        }
                }               
        } // */

        /* use data from extrinsics file
        if (globalCameraInfo.cameraMatrix.rows == 3) {
                for (unsigned int iii = 0; iii < 3; iii++) {
                        for (unsigned int jjj = 0; jjj < 3; jjj++) {
                            if (configData.camera_number==0){
                                camera_info.K[iii*3 + jjj] = globalExtrinsicsData.cameraMatrix0.at<double>(iii,jjj);
                            }else if(configData.camera_number==1){
                                camera_info.K[iii*3 + jjj] = globalExtrinsicsData.cameraMatrix1.at<double>(iii,jjj);
                            }
                        }
                }
        } // */



        //HGH
        if (configData.wantsToAddExtrinsics){
            if (configData.camera_number == 0){
                for (unsigned int iii = 0; iii < 3; iii++) {
                    for (unsigned int jjj = 0; jjj < 3; jjj++) {
                        camera_info.R[iii*3 + jjj] = globalExtrinsicsData.R0.at<double>(iii,jjj);
                    }
                }
                for (unsigned int iii = 0; iii < 3; iii++) {
                    for (unsigned int jjj = 0; jjj < 4; jjj++) {
                        camera_info.P[iii*4 + jjj] = globalExtrinsicsData.P0.at<double>(iii,jjj);
                    }
                }
            }else if (configData.camera_number == 1){
                for (unsigned int iii = 0; iii < 3; iii++) {
                    for (unsigned int jjj = 0; jjj < 3; jjj++) {
                        camera_info.R[iii*3 + jjj] = globalExtrinsicsData.R1.at<double>(iii,jjj);
                    }
                }
                for (unsigned int iii = 0; iii < 3; iii++) {
                    for (unsigned int jjj = 0; jjj < 4; jjj++) {
                        camera_info.P[iii*4 + jjj] = globalExtrinsicsData.P1.at<double>(iii,jjj);
                    }
                }
            }

        } else {
                if (globalCameraInfo.R.rows == 3) {
                        for (unsigned int iii = 0; iii < 3; iii++) {
                                for (unsigned int jjj = 0; jjj < 3; jjj++) {
                                        camera_info.R[iii*3 + jjj] = globalCameraInfo.R.at<double>(iii,jjj);
                                }
                        }
                }

                if (globalCameraInfo.newCamMat.rows == 3) {
                        for (unsigned int iii = 0; iii < 3; iii++) {
                                for (unsigned int jjj = 0; jjj < 3; jjj++) {
                                            camera_info.P[iii*4 + jjj] = globalCameraInfo.newCamMat.at<double>(iii,jjj);
                                }
                        }
                }
        }


        if (configData.wantsToUndistort) {
                if ((globalCameraInfo.cameraMatrix.rows == 3) && (globalCameraInfo.distCoeffs.cols > 0) && (globalCameraInfo.newCamMat.rows == 3)) {
                    //HGH initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.R, globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
                    //HGH
                    if (configData.wantsToAddExtrinsics){
                        if (configData.wantsToRectify){
                            if (configData.camera_number == 0){
                                initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalExtrinsicsData.R0, globalExtrinsicsData.P0, globalCameraInfo.cameraSize, CV_32FC1,  map1, map2);
                            }else if (configData.camera_number == 1){
                                initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalExtrinsicsData.R1, globalExtrinsicsData.P1, globalCameraInfo.cameraSize, CV_32FC1,  map1, map2);
                            }
                        }else{
                            initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs,  Mat(), globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
                        }
                    }else{
                        initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs,  Mat(), globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
                    }

                }
        }
        
        ROS_INFO("Camera info assigned.");
        
}

//HGH
void streamerNode::updateCameraInfoExtrinsics() {
    ROS_INFO("Updating camera info...");
    if (configData.camera_number == 0){
        for (unsigned int iii = 0; iii < 3; iii++) {
            for (unsigned int jjj = 0; jjj < 4; jjj++) {
                camera_info.P[iii*4 + jjj] = globalExtrinsicsData.P0.at<double>(iii,jjj);
            }
        }
    }else if (configData.camera_number == 1){
        for (unsigned int iii = 0; iii < 3; iii++) {
            for (unsigned int jjj = 0; jjj < 4; jjj++) {
                camera_info.P[iii*4 + jjj] = globalExtrinsicsData.P1.at<double>(iii,jjj);
            }
        }
    }
    ROS_INFO("Camera info updated.");
}

bool streamerNode::setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp) {

        camera_info = req.camera_info;

        if (configData.verboseMode) { ROS_INFO("calling assignCameraInfo() from setCameraInfo()..."); }
        assignCameraInfo();

        if (camera_calibration_parsers::writeCalibrationIni("../camera_parameters.txt", "gscam", camera_info)) {
                ROS_INFO("Camera information written to camera_parameters.txt");
                return true;
        }
        else {
                ROS_ERROR("Could not write camera_parameters.txt");
                return false;
        }
}

bool streamerNode::streamCallback(bool capture) {
        
        int currAttempts = 0;
        
        if (configData.verboseMode){ ROS_INFO("Entered callback."); }
        if ((configData.inputDatatype == DATATYPE_8BIT) || (configData.inputDatatype == DATATYPE_MM)) {
                if (configData.verboseMode){ ROS_INFO("Updating camera info..."); }
                updateCameraInfo();
                if (configData.verboseMode){ ROS_INFO("Camera info updated."); }
                ofs_call_log << ros::Time::now().toNSec() << endl;
                if (configData.verboseMode){ ROS_INFO("Capturing frame (8-bit/MM)..."); }
                cap >> frame;
                if (configData.verboseMode){ ROS_INFO("Frame captured."); }
                ofs_retrieve_log << ros::Time::now().toNSec() << endl;
        } else if (configData.inputDatatype == DATATYPE_RAW) {
                
                ros::Time callTime, retrieveTime;
                
                callTime = ros::Time::now();
                if (configData.verboseMode){ ROS_INFO("Capturing frame (16-bit)..."); }
                
                bool frameRead = false;
                while ((configData.maxReadAttempts == 0) || (currAttempts <= configData.maxReadAttempts)) {
                        // Keep on looping, but if max defined, only until curr attempts is high enough
                        
                        if (configData.verboseMode){ ROS_INFO("Attempting to capture frame..."); }
                        
                        if (av_read_frame(mainVideoSource->pIFormatCtx, &(mainVideoSource->oPacket)) != 0) {
                                if (configData.verboseMode){ ROS_WARN("av_read_frame() failed."); }
                                currAttempts++;
                                continue;
                        }
                        
                        if (configData.verboseMode){ ROS_INFO("Frame captured successfully."); }
                        
                        retrieveTime = ros::Time::now();
                        firmwareTime = mainVideoSource->oPacket.pts;
                        
                        if (configData.verboseMode){ ROS_INFO("Updating camera info..."); }
                        updateCameraInfo();
                        if (configData.verboseMode){ ROS_INFO("Camera info updated."); }
                        
                        if (mainVideoSource->bRet < 0) {
                                if (configData.verboseMode) { ROS_WARN("(mainVideoSource->bRet < 0) failed."); }
                                currAttempts++;
                                continue;
                        }
                        
                        if (mainVideoSource->oPacket.stream_index != mainVideoSource->ixInputStream) {
                                if (configData.verboseMode) { ROS_WARN("(mainVideoSource->oPacket.stream_index != mainVideoSource->ixInputStream) failed."); }
                                currAttempts++;
                                continue;
                        }
                        
                        if (configData.verboseMode){ ROS_INFO("Decoding frame..."); }
                        avcodec_decode_video2(mainVideoSource->pICodecCtx, mainVideoSource->pFrame, &(mainVideoSource->fFrame),&(mainVideoSource->oPacket));
                        if (configData.verboseMode){ ROS_INFO("Frame decoded."); }
                        
                        av_free_packet(&(mainVideoSource->oPacket));
                        if (configData.verboseMode){ ROS_INFO("Frame freed."); }
                        
                        if (!(mainVideoSource->fFrame)) {
                                if (configData.verboseMode) { ROS_WARN("(!(mainVideoSource->fFrame)) failed."); }
                                currAttempts++;
                                continue;
                        }
                        
                        if (capture) {
                                if (configData.verboseMode){ ROS_INFO("Accepting image..."); }
                                acceptImage((void*) *(mainVideoSource->pFrame->data));
                                if (configData.verboseMode){ ROS_INFO("Image accepted."); }
                                
                                char outbuff[256];
                                
                                uint32_t internal_sec, internal_nsec;
                
                                internal_sec = firmwareTime / 1000000;
                                internal_nsec = (firmwareTime % 1000000) * 1000;
                                
                                //sprintf(outbuff, "%016d000", firmwareTime);
                                sprintf(outbuff, "%010d.%09d", internal_sec, internal_nsec);
                                
                                if (configData.wantsToDumpTimestamps) {
                                        ofs_internal_log << outbuff << endl;
                                        //ofs_retrieve_log << retrieveTime.toNSec() << endl;
                                        //ofs_retrieve_log << retrieveTime.sec << "." << retrieveTime.nsec << endl;
                                        char output_time[256];
                                        sprintf(output_time,"%010d.%09d", retrieveTime.sec, retrieveTime.nsec);
                                        ofs_retrieve_log << output_time << endl;
                                        ofs_call_log << callTime.toNSec() << endl;
                                }
                                
                        }
                        
                        // Want to exit loop if it actually worked
                        if (configData.verboseMode){ ROS_INFO("Frame read."); }
                        break;
                        
                }
                
                if (currAttempts >= 1) {
                        ROS_WARN("Frame reading required (%d) additional attempt.", currAttempts);
                }

                if (currAttempts > configData.maxReadAttempts)  {
                        ROS_ERROR("Frame reading failed after (%d) attempts.", currAttempts);
                        setValidity(false);
                        return false;
                }
        }
        
        // processImage();
        
        //ROS_INFO("Exiting callback.");
        if (configData.verboseMode){ ROS_INFO("Exiting callback..."); }
        
        return true;
}

void streamerNode::acceptImage(void *ptr) {
        
        if (configData.inputDatatype == DATATYPE_RAW) {
                if ((frame.rows == 0) || (frame.type() != CV_16UC1)) {
                        frame = Mat::zeros(camera_info.height, camera_info.width, CV_16UC1);
                }
                
                if (configData.verboseMode){ ROS_INFO("Copying image data to internal matrix... (%d, %d)", camera_info.width, camera_info.height); }
                memcpy(&(frame.at<unsigned char>(0,0)), ptr, camera_info.width*camera_info.height*2);
                if (configData.verboseMode){ ROS_INFO("Data copied."); }
                
                if (configData.fixDudPixels) {
                        if (configData.verboseMode){ ROS_INFO("Fixing dud pixels..."); }
                        fix_bottom_right(frame);
                        if (configData.verboseMode){ ROS_INFO("Dud pixels fixed."); }
                }
                
        } else if (configData.inputDatatype == DATATYPE_8BIT) {
                if ((frame.rows == 0) || (frame.type() != CV_8UC1)) {
                        frame = Mat::zeros(camera_info.height, camera_info.width, CV_8UC1);
                }
                
                if (configData.verboseMode){ ROS_INFO("Copying image data to internal matrix.... (%d, %d)", camera_info.width, camera_info.height); }
                memcpy(&(frame.at<unsigned char>(0,0)), ptr, camera_info.width*camera_info.height);
                if (configData.verboseMode){ ROS_INFO("Data copied."); }
                
        } else if (configData.inputDatatype == DATATYPE_MM){
                if ((frame.rows == 0) || (frame.type() != CV_8UC3)) {
                        frame = Mat::zeros(camera_info.height, camera_info.width, CV_8UC3);
                }
                
                if (configData.verboseMode){ ROS_INFO("Copying image data to internal matrix... (%d, %d)", camera_info.width, camera_info.height); }
                memcpy(&(frame.at<unsigned char>(0,0)), ptr, camera_info.width*camera_info.height*3);
                if (configData.verboseMode){ ROS_INFO("Data copied."); }
        }

        
        
}

bool streamerNode::processImage() {
        
        if (frame.rows == 0) {
                return false;
        }
        
        if (configData.wantsToResize) {
                resize(frame, rzMat, Size(configData.desiredCols, configData.desiredRows));
                frame = Mat(rzMat);
        }
        
        if (configData.wantsToRemoveDuplicates || configData.wantsToMarkDuplicates || configData.wantsToOutputDuplicates) {
                
                if (matricesAreEqual(frame, lastFrame)) {               
                        
                        if (configData.wantsToMarkDuplicates) {
                                markCurrentFrameAsDuplicate();
                        }
                        
                        //ROS_INFO("SHOULD GET HERE A.");
                        if (configData.wantsToOutputDuplicates) {
                                //ROS_INFO("IS IT WRITING HERE? A");
                                ofs_duplicates_log << "1" << endl;
                        } else {
                                //ROS_INFO("WHY HERE? A");
                        }
                        //
                        
                        if (configData.wantsToRemoveDuplicates) {
                                if (configData.loadMode) {
                                        frameCounter++;
                                }
                                return false;
                        }
                        
                } else {
                        //ROS_ERROR("Not omitting...");
                        
                        //ROS_INFO("SHOULD GET HERE.");
                        if (configData.wantsToOutputDuplicates) {
                                //ROS_INFO("IS IT WRITING HERE?");
                                ofs_duplicates_log << "0" << endl;
                        } else {
                                //ROS_INFO("WHY HERE? B");
                        }
                        
                }
                
                frame.copyTo(lastFrame);
                
        }

        
        if (configData.readThermistor) {
                //ROS_WARN("Writing thermistor value... (%f)", lastThermistorReading);
                //ofs_thermistor_log << lastThermistorReading << endl;
        }
        

        //HGH
         //ROS_INFO("Processing image (%d)...", frameCounter);
        if (configData.verboseMode){ROS_INFO("Processing image (%d)...", frameCounter);}

        
        if (pastMeanIndex >= (configData.maxPastMeans-1)) {
                pastMeanIndex = 0;
        } else {
                pastMeanIndex++;
        }
        
        if (configData.inputDatatype == DATATYPE_RAW) {
                
                _16bitMat = Mat(frame);

                
                if ((configData.outputColorFlag) || (configData.output8bitFlag) || ((configData.wantsToWrite) && ((configData.outputType == "CV_8UC3") || (configData.outputType == "CV_8UC1")) )) {
                        //printf("%s << Applying adaptiveDownsample ... (%d)\n", __FUNCTION__, configData.normMode);
                        
                        adaptiveDownsample(_16bitMat, preFilteredMat, configData.normMode, configData.normFactor); //, configData.filterMode);
                        
                }

                
        } else if (configData.inputDatatype == DATATYPE_8BIT) {

                
                //ROS_WARN("Entered.. 2");
                
                if (frame.channels() == 3) {
                        
                        //ROS_WARN("Entered.. X");
                        
                        if (firstFrame) {
                                isActuallyGray = checkIfActuallyGray(frame);
                                firstFrame = false;
                                
                                if (configData.forceInputGray) {
                                        isActuallyGray = true;
                                        ROS_WARN("Forcing input images to be considered gray.");
                                }
                        }
                        
                }
                
                if ((frame.channels() == 1) || (isActuallyGray)) {
                        
                        if (frame.channels() == 1) {
                                workingFrame = Mat(frame);
                        } else if (isActuallyGray) {
                                if (((configData.outputColorFlag) || (configData.outputType == "CV_8UC3")) || ((configData.output8bitFlag) || (configData.outputType == "CV_8UC1"))) {
                                        cvtColor(frame, workingFrame, CV_RGB2GRAY);
                                }
                        }

                        //ROS_WARN("Processing...");
                        
                        // Need to normalize if appropriate
                        process8bitImage(workingFrame, preFilteredMat, configData.normMode, configData.normFactor);
                        
                } else if (frame.channels() == 3) {
                        
                        colourMat = Mat(frame);
                        
                        if ((configData.output8bitFlag) || (configData.outputType == "CV_8UC1")) {
                                cvtColor(colourMat, preFilteredMat, CV_RGB2GRAY);
                        }
                        
                }

        } else if (configData.inputDatatype == DATATYPE_MM) {
                
                //ROS_WARN("Entered.. 3");
                
                if (frame.channels() != 3) {
                        
                        ROS_ERROR("Frames must have 3 channels to be used for multi-modal fusion.");
                        
                        if (firstFrame) {
                                isActuallyGray = false;
                                firstFrame = false;
                        }
                        
                } else {
                        
                        //ROS_ERROR("Processing 3-channel image as an MM...");
                
                        // This is where you'll break the image apart and perform fusion...
                        
                        Mat thermal, visible;
                        
                        splitMultimodalImage(frame, thermal, visible);
                        
                        double fusion_params[2];
                        fusion_params[0] = max(min(0.5, 0.5 - (fusionFactor/2.0)), 0.0);
                        fusion_params[1] = max(min(0.5, 1.0 - (fusionFactor/2.0)), 1.0);

                        colourMap.fuse_image(thermal, visible, colourMat, fusion_params);

                        
                        if ((configData.output8bitFlag) || (configData.outputType == "CV_8UC1")) {
                                cvtColor(colourMat, preFilteredMat, CV_RGB2GRAY);
                        }
                        
                }

        }
        
        if (configData.filterMode > 0) {
                applyFilter(preFilteredMat, smoothedMat, configData.filterMode, configData.filterParam);
        } else {
                smoothedMat = preFilteredMat;
        }
        
        if (configData.temporalSmoothing) {
                if (((configData.outputColorFlag) || ((configData.wantsToWrite) && (configData.outputType == "CV_8UC3"))) || ((configData.output8bitFlag) || ((configData.wantsToWrite) && (configData.outputType == "CV_8UC1")))) {
                        // If you want to output any kind of 8-bit format...
                        
                        Scalar means = mean(smoothedMat);
                
                        pastMeans[pastMeanIndex] = means[0];
                
                        //ROS_WARN("mean = (%f)", pastMeans[pastMeanIndex]);
                        
                        double temporalMean = 0.0;
                        
                        for (int iii = 0; iii < min(frameCounter+1, configData.maxPastMeans); iii++) {
                                
                                temporalMean += pastMeans[iii];
                                
                        }
                        
                        temporalMean /= min(frameCounter+1, configData.maxPastMeans);
                        
                        //ROS_WARN("temporalMean = (%f)", temporalMean);

                        // sign gives temporal vs mean
                        // 
                        shiftDiff = (((temporalMean - pastMeans[pastMeanIndex]) > 0.0) ? 1.0 : (((temporalMean - pastMeans[pastMeanIndex]) < 0.0) ? -1.0 : 0.0)) * min(abs((pastMeans[pastMeanIndex] - temporalMean)), configData.maxIntensityChange);
                        
                        _8bitMat = smoothedMat + shiftDiff;
                        
                        Scalar meanA = mean(_8bitMat);
                        //ROS_WARN("8-bit mean = (%f)", meanA[0]);
                        
                }
                
        } else {
                _8bitMat = smoothedMat;
        }

        if ((configData.inputDatatype != DATATYPE_MM) && ((configData.inputDatatype != DATATYPE_8BIT) || (frame.channels() != 3) || isActuallyGray)) {
        //if (1) {
                // If it wasn't a 3-channel 8-bit image to start with
                //ROS_ERROR("There...");
                //imshow("x", _8bitMat);
                //waitKey(1);
                
                if ((configData.outputColorFlag) || ((configData.wantsToWrite) && (configData.outputType == "CV_8UC3"))) {
                        // And if you want color output
                        
                        //ROS_INFO("Entered...");

                        colourMap.falsify_image(_8bitMat, colourMat, configData.mapParam);

                }
        }
        
        frameCounter++;
        
        if (!readyToPublish) {
                readyToPublish = true;
        }
        
        return true;

}

void streamerNode::initializeMessages() {
        
        
        if (configData.output16bitFlag) {
                if (msg_16bit.width == 0) {
                        if (_16bitMat.rows != 0) {
                                msg_16bit.width = _16bitMat.cols; 
                                msg_16bit.height = _16bitMat.rows;
                                msg_16bit.encoding = "mono16";
                                msg_16bit.is_bigendian = false;
                                msg_16bit.step = _16bitMat.cols*2;
                                msg_16bit.data.resize(_16bitMat.cols*_16bitMat.rows*2);
                        }                       
                }

        }
        
        if (configData.output8bitFlag) {
                if (msg_8bit.width == 0) {
                        if (_8bitMat.rows != 0) {
                                msg_8bit.width = _8bitMat.cols; 
                                msg_8bit.height = _8bitMat.rows;
                                msg_8bit.encoding = "mono8";
                                msg_8bit.is_bigendian = false;
                                msg_8bit.step = _8bitMat.cols*1;
                                msg_8bit.data.resize(_8bitMat.cols*_8bitMat.rows*1);
                        }
                }
        }
        
        if (configData.outputColorFlag) {
                if (msg_color.width == 0) {
                        if (colourMat.rows != 0) {
                                msg_color.width = colourMat.cols; 
                                msg_color.height = colourMat.rows;
                                msg_color.encoding = "bgr8";
                                msg_color.is_bigendian = false;
                                msg_color.step = colourMat.cols*3;
                                msg_color.data.resize(colourMat.cols*colourMat.rows*3);
                        }
                }
                
        }
}

void streamerNode::publishTopics() {


        initializeMessages();


        /*
        if (alphaChanged) {
                updateMap();
        }
        */

        bool cameraPublished = false;

        if (configData.wantsToDumpTimestamps) {
                //ROS_WARN("outputFile = (%s)", configData.outputTimeFile.c_str());
                ofs << camera_info.header.stamp.toNSec() << endl;
        }

        //cout << camera_info.header.stamp.toNSec() << endl;

        if ((configData.output16bitFlag) || (configData.wantsToWrite &&  (configData.outputType == "CV_16UC1"))) {

                //_16bitMat_pub.copyTo(testMat);

                if (configData.wantsToUndistort) {
                        if (_16bitMat.data == _16bitMat_pub.data) {
                                _16bitMat_pub = Mat(_16bitMat.size(), _16bitMat.type());
                        }

                        remap(_16bitMat, _16bitMat_pub, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0);
                } else {
                        _16bitMat_pub = Mat(_16bitMat);
                }

                //if (matricesAreEqual(publishTopics() testMat, _16bitMat_pub)) {
                        //ROS_ERROR("Violation!!");
                //} else {
                        //ROS_WARN("Non-Violation!!");
                //}

                if (configData.output16bitFlag) {

                        std::copy(&(_16bitMat_pub.at<char>(0,0)), &(_16bitMat_pub.at<char>(0,0))+(_16bitMat_pub.cols*_16bitMat_pub.rows*2), msg_16bit.data.begin());



                        if (!cameraPublished) {

                                pub_16bit.publish(msg_16bit, camera_info);
                                cameraPublished = true;
                        } else {

                                pub_16bit_im.publish(msg_16bit);
                        }


                        //HGH
                        if (configData.republishSource==REPUBLISH_CODE_16BIT){

                            if (configData.republishNewTimeStamp){
                                //republish with new time stamps
                                pub_republish.publish(msg_16bit, camera_info, ros::Time::now());
                            } else {
                                pub_republish.publish(msg_16bit, camera_info);
                            }
                        }

                }

        }

        if ((configData.output8bitFlag) || (configData.wantsToWrite &&  (configData.outputType == "CV_8UC1"))) {

                if (configData.wantsToUndistort) {
                        if (_8bitMat.data == _8bitMat_pub.data) {
                                _8bitMat_pub = Mat(_8bitMat.size(), _8bitMat.type());
                        }

                        remap(_8bitMat, _8bitMat_pub, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0);
                } else {
                        _8bitMat_pub = Mat(_8bitMat);
                }

                if (configData.output8bitFlag) {

                    //HGH
                    if (configData.drawReticle){
                        line(_8bitMat_pub, Point(0,_8bitMat_pub.rows/2), Point(_8bitMat_pub.cols, _8bitMat_pub.rows/2), Scalar(0),1,8);
                        line(_8bitMat_pub, Point(_8bitMat_pub.cols/2,0), Point(_8bitMat_pub.cols/2, _8bitMat_pub.rows), Scalar(0),1,8);
                    }

                        std::copy(&(_8bitMat_pub.at<unsigned char>(0,0)), &(_8bitMat_pub.at<unsigned char>(0,0))+(_8bitMat_pub.cols*_8bitMat_pub.rows), msg_8bit.data.begin());


                        if (!cameraPublished) {
                                pub_8bit.publish(msg_8bit, camera_info);

                                cameraPublished = true;
                        } else {
                                pub_8bit_im.publish(msg_8bit);

                        }

                        //HGH
                         if (configData.republishSource==REPUBLISH_CODE_8BIT_MONO){

                             if (configData.republishNewTimeStamp){
                                 //republish with new time stamps
                                 pub_republish.publish(msg_8bit, camera_info, ros::Time::now());
                             } else{
                                 pub_republish.publish(msg_8bit, camera_info);
                             }
                         }

                }

        }

        if ((configData.outputColorFlag) || (configData.wantsToWrite &&  (configData.outputType == "CV_8UC3"))) {


                if (colourMat.rows > 0) {


                        if (configData.wantsToUndistort) {
                                if (colourMat.data == colourMat_pub.data) {
                                        colourMat_pub = Mat(colourMat.size(), colourMat.type());
                                }

                                remap(colourMat, colourMat_pub, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0);
                        } else {
                                colourMat_pub = Mat(colourMat);
                        }

                        if (configData.outputColorFlag) {

                            //HGH
                            if (configData.drawReticle){
                                line(colourMat_pub, Point(0,colourMat_pub.rows/2), Point(colourMat_pub.cols, colourMat_pub.rows/2), Scalar(0,255,0),1,8);
                                line(colourMat_pub, Point(colourMat_pub.cols/2,0), Point(colourMat_pub.cols/2, colourMat_pub.rows), Scalar(0,255,0),1,8);
//                                //overlay lines:
//                                for( int j = 0; j < colourMat_pub.rows; j += 32 ){
//                                    line(colourMat_pub, Point(0, j), Point(colourMat_pub.cols, j), Scalar(0, 255, 0), 1, 8);
//                                }
                            }

                                std::copy(&(colourMat_pub.at<Vec3b>(0,0)[0]), &(colourMat_pub.at<Vec3b>(0,0)[0])+(colourMat_pub.cols*colourMat_pub.rows*3), msg_color.data.begin());

                                if (!cameraPublished) {
                                        pub_color.publish(msg_color, camera_info);
                                        cameraPublished = true;
                                } else {
                                        pub_color_im.publish(msg_color);
                                }

                                //HGH
                                 if (configData.republishSource==REPUBLISH_CODE_8BIT_COL){

                                     if (configData.republishNewTimeStamp){
                                         //republish with new time stamps
                                         pub_republish.publish(msg_color, camera_info, ros::Time::now());
                                     }else{
                                         pub_republish.publish(msg_color, camera_info);
                                     }

                                 }
                        }


                        /*
                        imshow("display", colourMat);
                        waitKey(1);
                        */
                }
        }



        readyToPublish = false;

}

void streamerNode::writeData() {
        
        if (configData.wantsToWrite) {
                
                //ROS_ERROR("wantsToWrite == true");
                                
                if ((frameCounter-1) != lastWritten) {
                        
                        //ROS_ERROR("Entered write routine...");
                
                        char *outputFilename;
                        outputFilename = (char*) malloc(256);
                        
                        if (configData.loadMode && configData.wantsToKeepNames) {
                                
                                size_t findDot = inputList.at(frameCounter-1).rfind(".");
                                
                                string partialName;
                                partialName = inputList.at(frameCounter-1).substr(0, findDot);
                                
                                sprintf(outputFilename, "%s/%s.%s", configData.outputFolder.c_str(), partialName.c_str(), configData.outputFormatString.c_str());
                                
                        } else {
                                sprintf(outputFilename, "%s/frame%06d.%s", configData.outputFolder.c_str(), frameCounter-1, configData.outputFormatString.c_str());
                        }
                        
                        // ROS_ERROR("Output name = (%s)", outputFilename);
                        
                        if (configData.outputType == "CV_16UC1") {
                                if (_16bitMat.rows > 0) {
                                        
                                        if (configData.outputFormatString == "png") {
                                                imwrite(outputFilename, _16bitMat_pub, configData.outputFileParams);
                                        } else if ((configData.outputFormatString == "pgm") || (configData.outputFormatString == "ppm")) {
                                                imwrite(outputFilename, _16bitMat_pub);
                                        }
                                }
                        } else if (configData.outputType == "CV_8UC3") {
                                if (colourMat.rows > 0) {
                                        
                                        //ROS_ERROR("Actually writing...");
                                        
                                        if ((configData.outputFormatString == "png") || (configData.outputFormatString == "jpg")) {
                                                imwrite(outputFilename, colourMat_pub, configData.outputFileParams);
                                        } else if ((configData.outputFormatString == "bmp") || (configData.outputFormatString == "ppm")) {
                                                imwrite(outputFilename, colourMat_pub);
                                        }
                                }
                        } else if (configData.outputType == "CV_8UC1") {
                                if (_8bitMat.rows > 0) {
                                        if ((configData.outputFormatString == "png") || (configData.outputFormatString == "jpg")) {
                                                imwrite(outputFilename, _8bitMat_pub, configData.outputFileParams);
                                        } else if ((configData.outputFormatString == "bmp") || (configData.outputFormatString == "pgm") || (configData.outputFormatString == "ppm")) {
                                                imwrite(outputFilename, _8bitMat_pub);
                                        }
                                }
                        }                       
                        lastWritten = frameCounter - 1;
                }
        }
        
        if (configData.wantsToEncode) {
                if (!videoInitialized) {
                        if (configData.videoType == "CV_16UC1") {
                                // 0 writes uncompressed, 1 gives user option
                                vid_writer.open(configData.outputVideo, CV_FOURCC('P','I','M','1'), ((int) configData.framerate), _16bitMat.size(), true);
                        } else if (configData.videoType == "CV_8UC3") {
                                vid_writer.open(configData.outputVideo, CV_FOURCC('P','I','M','1'), ((int) configData.framerate), colourMat.size()); // , true);
                        } else if (configData.videoType == "CV_8UC1") {
                                vid_writer.open(configData.outputVideo, CV_FOURCC('X', 'V', 'I', 'D'), ((int) configData.framerate), _8bitMat.size(), false);
                        }
                        
                        videoInitialized = true;
                }
                
                if (configData.videoType == "CV_16UC1") {
                        vid_writer << _16bitMat;
                } else if (configData.videoType == "CV_8UC3") {
                        vid_writer << colourMat;
                } else if (configData.videoType == "CV_8UC1") {
                        vid_writer << _8bitMat;
                }

        }
}

void streamerNode::serialCallback(const ros::TimerEvent&) {
        
        if (!configData.serialComms) {
                return;
        }
        
        if (configData.serialPollingRate == 0.0) {
                ROS_WARN("Skipping serial callback because desired frequency is zero (0).");
                return;
        } else {
                //ROS_WARN("Polling serial device...");
                
                if (configData.readThermistor) {
                        
                        newThermistorReading = getThermistorReading();
                        
                        //ROS_INFO("newVal = (%f) vs oldVal = (%f)", newThermistorReading, lastThermistorReading);
                        
                        // If new value is valid
                        if ((newThermistorReading > -20.0) && (newThermistorReading < 60.0)) {
                                
                                // If the historical value is valid
                                if (lastThermistorReading != 0.0) {
                                        
                                        
                                        if (abs(newThermistorReading - lastThermistorReading) < configData.maxThermistorDiff) {
                                                lastThermistorReading = newThermistorReading;
                                        }
                                        
                                        
                                } else {
                                        lastThermistorReading = newThermistorReading;
                                }
                                
                        }
                        
                        //ROS_WARN("Thermistor value = (%f)", lastThermistorReading);
                }
                
                if (configData.calibrationMode > 0) {
                        if (alternateCounter >= configData.alternatePeriod) {
                                
                                if (configData.calibrationMode == CALIBMODE_ALT_OUTPUT) {
                                        if (altStatus) {
                                                sprintf(serialCommand, "vidout raw\r");
                                        } else {
                                                sprintf(serialCommand, "vidout ins\r");
                                        }
                                        altStatus = !altStatus;
                                } else if (configData.calibrationMode == CALIBMODE_ALT_SHUTTER) {
                                        if (performingNuc) {
                                                
                                                if (configData.verboseMode) { ROS_INFO("About to switch shutter"); }
                                                
                                                if (altStatus) {
                                                        sprintf(serialCommand, "close\r");
                                                } else {
                                                        sprintf(serialCommand, "open\r");
                                                }
                                                altStatus = !altStatus;
                                                
                                        } else {
                                                ROS_INFO("About to perform a NUC");
                                                sprintf(serialCommand, "nuc\r");
                                        }
                                        
                                        performingNuc = !performingNuc;
                                } else if (configData.calibrationMode == CALIBMODE_LONG_SHUTTER) {
                                        
                                        if (configData.verboseMode) { ROS_INFO("About to switch shutter"); }
                                        
                                        if (altStatus) {
                                                sprintf(serialCommand, "close\r");
                                        } else {
                                                sprintf(serialCommand, "open\r");
                                        }
                                        
                                        altStatus = !altStatus;
                                }
                                
                                int bytesToWrite = 0;
                        
                                while (serialCommand[bytesToWrite] != '\r') {
                                        bytesToWrite++;
                                }
                                bytesToWrite++;
                                
                                int n;
                                //char buff[256];
                                
                                ROS_INFO("Sending NUCSET command: (%d) %s", bytesToWrite, serialCommand);
                                n = write(mainfd, serialCommand, bytesToWrite);          
                                if (n!=bytesToWrite) puts("write() failed!\n");

                                
                                
                                alternateCounter = 0;
                        }
                        alternateCounter++;
                }
                
        }
        
        if (updateDetectorMode) {
                
                string detectorCode;
                
                if (configData.detectorMode == DETECTOR_MODE_RAW) {
                        detectorCode = "raw";
                } else if (configData.detectorMode == DETECTOR_MODE_LUM) {
                        detectorCode = "lum";
                } else if (configData.detectorMode == DETECTOR_MODE_INS) {
                        detectorCode = "ins";
                } else if (configData.detectorMode == DETECTOR_MODE_RAD) {
                        detectorCode = "rad";
                } else if (configData.detectorMode == DETECTOR_MODE_TMP) {
                        detectorCode = "t";
                }
                sprintf(serialCommand, "vidout %s\r", detectorCode.substr(0,3).c_str());
                
                int bytesToWrite = 1;
                
                while (serialCommand[bytesToWrite-1] != '\r') {
                        bytesToWrite++;
                }
                                
                ROS_INFO("Changing detector output with command: <%s> (%d bytes) ", string(serialCommand).substr(0,bytesToWrite-1).c_str(), bytesToWrite);
                
                int n;
                char buff[256];
                n = write(mainfd, serialCommand, bytesToWrite);          
                if (n!=bytesToWrite) {
                        ROS_ERROR("write() failed!");
                }
                n = read(mainfd, buff, 256);
                
                // ROS_INFO("Returned is <%s>", buff);
                
                updateDetectorMode = false;
        }
        
        if (updateNucInterval) {
                
                // Value is in 100ths of a degree centigrade, so pass 20 to trigger change every 0.2 C
                sprintf(serialCommand, "nucset %d %d %d %d\r", configData.maxNucInterval, 0, 0, int(100*configData.maxNucThreshold));
                
                int bytesToWrite = 0;
                
                while (serialCommand[bytesToWrite] != '\r') {
                        bytesToWrite++;
                }
                bytesToWrite++;

                
                ROS_INFO("Sending NUCSET command: (%d) %s", bytesToWrite, serialCommand);
                
                
                
                //sprintf(serialCommand, "nucset\r");
                
                // Send new NUCSET command
                
                int n;
                char buff[256];
                n = write(mainfd, serialCommand, bytesToWrite);          
                if (n!=bytesToWrite) puts("write() failed!\n");
                n = read(mainfd, buff, 256);
                
                ROS_INFO("Returned is (%s)", buff);
                
                updateNucInterval = false;
        }
        
}

void streamerNode::timerCallback(const ros::TimerEvent&) {
        
        if (configData.pauseMode) {
                return;
        }
        
        if (configData.captureMode || configData.subscribeMode) {
                return;
        }
        
        if (configData.pollMode || configData.resampleMode) {
                
                if (configData.pollMode) {
                        
                        //ROS_ERROR("About to call stream");
                        
                        streamCallback();
                        
                        //ROS_ERROR("Stream called");
                }
                
                if (processImage()) {
                        
                        //ROS_ERROR("Processed");
                        
                        if (readyToPublish) {
                                if (configData.resampleMode) {
                                        //updateCameraInfo();
                                }
                                publishTopics();
                                
                                //ROS_ERROR("Published");
                                
                                writeData();
                                
                                //ROS_ERROR("Written");
                                
                        }
                        
                }
                
                //ROS_ERROR("All done.");
                

                return;
        }
        
        if (configData.loadMode) {
                
                string filename;
                
                if (configData.folder.at(configData.folder.length()-1) == '/') {
                        filename = configData.folder + inputList.at(frameCounter);
                } else {
                        filename = configData.folder + "/" + inputList.at(frameCounter);
                }

                frame = imread(filename, -1);
                
                //ROS_INFO("Read frame (%s)", filename.c_str());
                
                if (processImage()) {
                        //ROS_INFO("Image process is true...");
                        if (readyToPublish) {
                                //ROS_INFO("Publishing...");
                                updateCameraInfo();
                                publishTopics();
                                writeData();
                        }
                        
                }
                
                if (frameCounter >= fileCount) {
                        if (configData.verboseMode) { ROS_WARN("setValidity(false) : (frameCounter >= fileCount)"); }
                        setValidity(false);
                }
                
                return;
        }

        bool validFrameRead = true;
        
        // If in read mode...
        if (configData.inputDatatype == DATATYPE_RAW) {

                do { 
                        //ROS_WARN("Attempting to read frame...");
                        if (av_read_frame(mainVideoSource->pIFormatCtx, &(mainVideoSource->packet)) != 0) {
                                ROS_WARN("Frame invalid...");
                                validFrameRead = false;
                                break;
                        }
                        //ROS_WARN("Frame valid!");
                } while (mainVideoSource->packet.stream_index != mainVideoSource->videoStream);

                
                if (validFrameRead) {

                        
                        avcodec_decode_video2(mainVideoSource->pCodecCtx, mainVideoSource->pFrame, &(mainVideoSource->frameFinished), &(mainVideoSource->packet));

                        acceptImage((void*) *(mainVideoSource->pFrame->data));

                        if (processImage()) {
                                if (readyToPublish) {
                                        updateCameraInfo();
                                        publishTopics();
                                        writeData();
                                }
                        }

                        

                        av_free_packet(&(mainVideoSource->packet));
                        
                } else {
                        if (configData.verboseMode) { ROS_WARN("setValidity(false) : (validFrameRead)"); }
                        setValidity(false);
                }
                
        } else if ((configData.inputDatatype == DATATYPE_8BIT) || (configData.inputDatatype == DATATYPE_MM)) {
                
                ROS_WARN("About to read in frame...");
                
                if(!cap.isOpened()) {
                        ROS_ERROR("Actually, device isn't open...");
                } else {
                        ROS_INFO("Device is open...");
                }
                
                ROS_INFO("framecount = (%f)", cap.get(CV_CAP_PROP_FRAME_COUNT));
                
                ROS_INFO("dim = (%f, %f)", cap.get(CV_CAP_PROP_FRAME_WIDTH), cap.get(CV_CAP_PROP_FRAME_HEIGHT));
                
                cap >> frame;
                //IplImage* tmp_im = cvQueryFrame(capture);
                //frame = tmp_im;
                
                ROS_WARN("Frame read");
                
                //imshow("test", frame);
                //waitKey();
                
                if (&frame == NULL) {
                        if (configData.verboseMode) { ROS_WARN("setValidity(false) : (&frame == NULL)"); }
                        setValidity(false);
                        
                } else {
                        if (processImage()) {
                                if (readyToPublish) {
                                        updateCameraInfo();
                                        publishTopics();
                                        writeData();
                                }
                        }


                }
                
        }
}

void streamerNode::assignDefaultCameraInfo() {
        
        globalCameraInfo.imageSize = Mat(1, 2, CV_16UC1);
        globalCameraInfo.imageSize.at<unsigned short>(0, 1) = DEFAULT_IMAGE_HEIGHT;
        globalCameraInfo.imageSize.at<unsigned short>(0, 0) = DEFAULT_IMAGE_WIDTH;
        
        globalCameraInfo.cameraMatrix = Mat::eye(3, 3, CV_64FC1);
        globalCameraInfo.distCoeffs = Mat::zeros(1, 8, CV_64FC1);
        globalCameraInfo.newCamMat = Mat::eye(3, 3, CV_64FC1);
        globalCameraInfo.cameraSize = Size(globalCameraInfo.imageSize.at<unsigned short>(0, 0), globalCameraInfo.imageSize.at<unsigned short>(0, 1));
        
}

void streamerNode::overwriteCameraDims() {
        
        camera_info.height = globalCameraInfo.imageSize.at<unsigned short>(0, 1);
        camera_info.width = globalCameraInfo.imageSize.at<unsigned short>(0, 0);
        
}

streamerNode::streamerNode(ros::NodeHandle& nh, streamerData startupData) {
        
        sprintf(nodeName, "%s", ros::this_node::getName().substr(1).c_str());
        
        ROS_INFO("Initializing node (%s)", nodeName);
        
        firstCall = true;
        updateNucInterval = true;
        updateDetectorMode = true;
        altStatus = true;
        performingNuc = false;
        alphaChanged = true;
        readyToPublish = false;
        isActuallyGray = false;
        videoInitialized = false;
        setValidity(true);
        firstFrame = true;
    centerPrincipalPoint = true;
        firstServerCallbackProcessed = false;
        pastMeanIndex = -1;
        medianPercentile = 0.50;
        alternateCounter = 0;
        dodgeTime.sec = 0;
        dodgeTime.nsec = 0;
        fusionFactor = 0.8;
        fileCount = 0;
        writeIndex = 0;
        frameCounter = 0;
        lastWritten = -1;
        lastThermistorReading = 0.0;
        lastMedian = -1.0;
        lastLowerLimit = -1.0;
        lastUpperLimit = -1.0;
        oldMaxDiff = -1.0;
        minVal = 65535.0;
        maxVal = 0.0;
        
        configData = startupData;
        
        if (configData.serialComms) {
                ROS_INFO("Configuring serial comms...");
                ROS_INFO("Serial comms configured.");
                if (!configureSerialComms()) {
                        if (configData.verboseMode) { ROS_WARN("Serial comms configuration failed. Shutting down."); }
                        wantsToShutdown = true;
                        prepareForTermination();
                        return;
                }
        }
        
        ROS_INFO("configData.serialPollingRate = (%f)", configData.serialPollingRate);
        
        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)), &streamerNode::serialCallback, this);
        } else {
                ROS_INFO("Initializing serial timer at (%f)", DEFAULT_SERIAL_POLLING_RATE);
                serial_timer = nh.createTimer(ros::Duration(DEFAULT_SERIAL_POLLING_RATE), &streamerNode::serialCallback, this);
        }
        
        it = new image_transport::ImageTransport(nh);
        
        string info_name = configData.topicname.substr(0, configData.topicname.find_last_of("/") + 1) + "camera_info";
        ROS_INFO("configData.topicname = (%s)", info_name.c_str());
        
        if (configData.calibrationMode) {
                ros::Duration(1.0).sleep(); // wait a little bit to ensure that the serial comms has been configured..
        }
        
        ROS_INFO("Initializing camera (%s)", configData.topicname.c_str());
        
        mainVideoSource = new streamerSource;

        std::string camera_name;
        
        if (configData.outputFolder.size() == 0) {
                configData.outputFolder = configData.read_addr + configData.outputFolder;
        } else if (configData.outputFolder[0] != '/') {
                configData.outputFolder = configData.read_addr + configData.outputFolder;
        }
        
        configData.outputTimeFile = configData.outputFolder + "-timestamps.txt";
        
        if (configData.wantsToDumpTimestamps) {
                ofs.open(configData.outputTimeFile.c_str());
        }
        
        if (configData.intrinsicsProvided) {
                
                ROS_INFO("Reading in calibration data");
                
                if (configData.intrinsics[0] != '/') {
                        configData.intrinsics = configData.read_addr + configData.intrinsics;
                }
                
                ROS_INFO("intrinsics = %s", configData.intrinsics.c_str());
                
                // http://www.mathpirate.net/log/2009/11/26/when-in-doubt-link-debug/
                Mat dummyImage(300, 300, CV_8UC1);
                GaussianBlur(dummyImage, dummyImage, Size(5, 5), 1.5);
                
                
                FileStorage fs(configData.intrinsics, FileStorage::READ);
                fs["imageSize"] >> globalCameraInfo.imageSize;
                fs["cameraMatrix"] >> globalCameraInfo.cameraMatrix;
                fs["distCoeffs"] >> globalCameraInfo.distCoeffs;
                fs["newCamMat"] >> globalCameraInfo.newCamMat;
                fs.release();
                
                ROS_INFO("Calibration data read.");

                        //HGH
                        if (globalCameraInfo.cameraMatrix.empty()){
                                ROS_ERROR("Intrinsics file %s invalid! Please check path and filecontent...\n", configData.intrinsics.c_str());
                        }
                
                ROS_INFO("Establishing size (%d, %d).", globalCameraInfo.imageSize.at<unsigned short>(0, 0), globalCameraInfo.imageSize.at<unsigned short>(0, 1));
                
                globalCameraInfo.cameraSize = Size(globalCameraInfo.imageSize.at<unsigned short>(0, 0), globalCameraInfo.imageSize.at<unsigned short>(0, 1));
                
                //globalCameraInfo.newCamMat = getOptimalNewCameraMatrix(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.cameraSize, configData.alpha, globalCameraInfo.cameraSize, validPixROI, centerPrincipalPoint);
                
                
                ROS_INFO("Global params determined.");
                
                
        } else if (configData.imageDimensionsSpecified) {
                
                ROS_INFO("Assigning default intrinsics but with specified image size");
                
                assignDefaultCameraInfo();
                
                globalCameraInfo.imageSize.at<unsigned short>(0, 1) = configData.inputHeight;
                globalCameraInfo.imageSize.at<unsigned short>(0, 0) = configData.inputWidth;
                globalCameraInfo.cameraSize = Size(globalCameraInfo.imageSize.at<unsigned short>(0, 0), globalCameraInfo.imageSize.at<unsigned short>(0, 1));
                
                // ROS_INFO("Assigned.");
                
        } else {
                ROS_INFO("Assigning default intrinsics...");
                
                assignDefaultCameraInfo();
                
                ROS_INFO("Default intrinsics assigned.");
        }
        
        
        //HGH
        if (configData.autoAlpha) {
                
                if (configData.intrinsicsProvided) {
                        if (configData.verboseMode) { ROS_INFO("Finding auto alpha..."); }
                        configData.alpha = findBestAlpha(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.cameraSize);
                        //HGH
                        ROS_INFO("Optimal alpha: (%f)", configData.alpha);
                } else {
                        ROS_WARN("Cannot estimate an appropriate alpha coefficient because no intrinsics were provided.");
                }
                
        }

        
        if (configData.wantsToAddExtrinsics) {
        ROS_INFO("Reading in extrinsics...");

                        char rotation_name[256], translation_name[256];

                        sprintf(rotation_name, "R_%d", configData.camera_number);
                        sprintf(translation_name, "T%d", configData.camera_number);

                        // http://www.mathpirate.net/log/2009/11/26/when-in-doubt-link-debug/
                        Mat dummyImage(300, 300, CV_8UC1);
                        GaussianBlur(dummyImage, dummyImage, Size(5, 5), 1.5);

        FileStorage fs(configData.extrinsics, FileStorage::READ);

                        //HGH
                        fs[rotation_name] >> globalCameraInfo.R;
                        fs[translation_name] >> globalCameraInfo.T;

                        //used to calculate the individual rotation and projection matrices for rectification depending on alpha
                        fs["R"] >> globalExtrinsicsData.R;
                        fs["T"] >> globalExtrinsicsData.T;

                        //we need also both camera matrices and distortion coefficients
                        fs["cameraMatrix0"] >> globalExtrinsicsData.cameraMatrix0;
                        fs["cameraMatrix1"] >> globalExtrinsicsData.cameraMatrix1;
                        fs["distCoeffs0"] >> globalExtrinsicsData.distCoeffs0;
                        fs["distCoeffs1"] >> globalExtrinsicsData.distCoeffs1;

        fs.release();

                        if (globalExtrinsicsData.R.empty()){
                                ROS_ERROR("Extrinsics file %s invalid! Please check path and filecontent...\n", configData.extrinsics.c_str());
                        }

                        globalCameraInfo.cameraSize = Size(globalCameraInfo.imageSize.at<unsigned short>(0, 0), globalCameraInfo.imageSize.at<unsigned short>(0, 1));

                        getRectification();

        } else {

                globalCameraInfo.R = Mat::eye(3, 3, CV_64FC1);
                globalCameraInfo.T = Mat::zeros(3, 1, CV_64FC1);
        }

        if (configData.verboseMode) { ROS_INFO("calling assignCameraInfo() from streamerNode()..."); }
        assignCameraInfo();

        ROS_INFO("Initializing video source...");
        mainVideoSource->initialize_video_source();

        colourMap.load_standard(configData.mapCode, configData.mapParam);
        
        refreshCameraAdvertisements();
        
        char cameraInfoName[256];
        sprintf(cameraInfoName, "thermalvis/%s/set_camera_info", nodeName);
        ros::ServiceServer set_camera_info = nh.advertiseService(cameraInfoName, &streamerNode::setCameraInfo, this);


        if (configData.framerate > 0.0) {
                configData.pauseMode = false;
                timer = nh.createTimer(ros::Duration(1.0 / ((double) configData.framerate)), &streamerNode::timerCallback, this);
        } else {
                configData.pauseMode = true;
                timer = nh.createTimer(ros::Duration(1.0 / 1.0), &streamerNode::timerCallback, this);
        }
        
        // Configure log files
        callLogFile = configData.read_addr + "/nodes/streamer/log/call_log.txt";
        retrieveLogFile = configData.read_addr + "/nodes/streamer/log/retrieve_log.txt";
        internalLogFile = configData.read_addr + "/nodes/streamer/log/internal_log.txt";
        writeLogFile = configData.read_addr + "/nodes/streamer/log/write_log.txt";
        duplicatesLogFile = configData.outputFolder + "-duplicates.txt";
        thermistorLogFile = configData.outputFolder + "-thermistor.txt";

        // &PGRCameraNode::publishImage, this, _1
        //f = boost::bind(&streamerNode::serverCallback, this, NULL);
        //f = boost::bind(serverCallback, _1, _2);
        //server.setCallback(f);
        
        ROS_INFO("Establishing server callback...");
        f = boost::bind (&streamerNode::serverCallback, this, _1, _2);
    server.setCallback (f);
    
    if (configData.subscribeMode || configData.resampleMode) {
                ROS_INFO("syncMode: (%d)", configData.syncMode);
                if (configData.syncMode == SYNCMODE_HARD) {
                        camera_sub = it->subscribeCamera(configData.topicname, 1, &streamerNode::handle_camera, this);
                } else if (configData.syncMode == SYNCMODE_SOFT) {
                        info_sub = nh.subscribe<sensor_msgs::CameraInfo>(info_name, 1, &streamerNode::handle_info, this);
                        image_sub = it->subscribe(configData.topicname, 1, &streamerNode::handle_image, this);
                } else if (configData.syncMode == SYNCMODE_IMAGEONLY) {
                        image_sub = it->subscribe(configData.topicname, 1, &streamerNode::handle_image, this);
                }
        }
    
    // Wait for the first server callback to be processed before continuing..
    while (!firstServerCallbackProcessed) { };
    
    if (configData.wantsToDumpTimestamps) {
                ofs_call_log.open(callLogFile.c_str());
                ofs_retrieve_log.open(retrieveLogFile.c_str());
                ofs_internal_log.open(internalLogFile.c_str());
                ofs_write_log.open(writeLogFile.c_str());
        }
        
        if (configData.wantsToOutputDuplicates) {
                ROS_ERROR("Outputting duplicates to (%s)\n", duplicatesLogFile.c_str());
                ofs_duplicates_log.open(duplicatesLogFile.c_str());
        }
}

//HGH
void streamerNode::getRectification(){

    Mat Q;

        stereoRectify(globalExtrinsicsData.cameraMatrix0, globalExtrinsicsData.distCoeffs0, globalExtrinsicsData.cameraMatrix1, globalExtrinsicsData.distCoeffs1,
                                  globalCameraInfo.cameraSize,
                                  globalExtrinsicsData.R, globalExtrinsicsData.T,
                                  globalExtrinsicsData.R0, globalExtrinsicsData.R1, globalExtrinsicsData.P0, globalExtrinsicsData.P1,
                                  Q,
                                  CALIB_ZERO_DISPARITY, configData.alpha, globalCameraInfo.cameraSize, &globalExtrinsicsData.roi0, &globalExtrinsicsData.roi1);

//        ROS_WARN("----- CAM Number %d",configData.camera_number);
//        cout << "R0 = " << globalExtrinsicsData.R0 << endl;
//        cout << "P0 = "  << globalExtrinsicsData.P0 << endl;
//        cout << "R1 = " << globalExtrinsicsData.R1 << endl;
//        cout << "P1 = "  << globalExtrinsicsData.P1 << endl;
//        cout << "newCamMat = " << globalCameraInfo.newCamMat << endl;
//        printf("roi0 (%d, %d, %d, %d) ", int(globalExtrinsicsData.roi0.x), int(globalExtrinsicsData.roi0.y), int(globalExtrinsicsData.roi0.width), int(globalExtrinsicsData.roi0.height));
//        printf("roi1 (%d, %d, %d, %d) ", int(globalExtrinsicsData.roi1.x), int(globalExtrinsicsData.roi1.y), int(globalExtrinsicsData.roi1.width), int(globalExtrinsicsData.roi1.height));
//        cout << "configData.alpha = " << configData.alpha << endl;

}

void streamerNode::handle_info(const sensor_msgs::CameraInfoConstPtr& info_msg) {
        
        
        if (configData.syncMode != SYNCMODE_SOFT) {
                return;
        }
        
        
        if ((!configData.subscribeMode) && (!configData.resampleMode)) {
                return;
        }
        
        info_time = ros::Time::now();
        original_time = info_msg->header.stamp;
        
        original_bx = info_msg->binning_x;
        original_by = info_msg->binning_y;
        
        //cout << "Handling info..." << info_msg->header.stamp.toNSec() << ", " << info_time.toNSec() << endl;
        
        if (abs(image_time.toNSec() - info_time.toNSec()) < configData.soft_diff_limit) {
                //ROS_ERROR("handle_info : image_time == info_time");
                image_time = dodgeTime;
                
                act_on_image();
        }
        
        
}

void streamerNode::refreshCameraAdvertisements() {


        char colorPubName[256], _16bitPubName[256], _8bitPubName[256];

        sprintf(colorPubName, "thermalvis/%s/image_col", nodeName);
        sprintf(_16bitPubName, "thermalvis/%s/image_raw", nodeName);
        sprintf(_8bitPubName, "thermalvis/%s/image_mono", nodeName);

        bool cameraAdvertised = false;

        // Once all copies of the returned Publisher object are destroyed, the topic
        // will be automatically unadvertised.

        pub_color.shutdown();
        pub_color_im.shutdown();
        pub_16bit.shutdown();
        pub_16bit_im.shutdown();
        pub_8bit.shutdown();
        pub_8bit_im.shutdown();

        //HGH
        pub_republish.shutdown();
        pub_republish_im.shutdown();

        char _republishPubName[256];
        sprintf(_republishPubName, configData.republishTopic.c_str());



        if (configData.output16bitFlag) {

            //HGH
            if (configData.republishSource==REPUBLISH_CODE_16BIT){
                pub_republish = it->advertiseCamera(_republishPubName, 1);
            }

                if (!cameraAdvertised) {
                        pub_16bit = it->advertiseCamera(_16bitPubName, 1);

                        cameraAdvertised = true;
                } else {
                        pub_16bit_im = it->advertise(_16bitPubName, 1);

                }

        }

        if (configData.output8bitFlag) {

            //HGH
            if (configData.republishSource==REPUBLISH_CODE_8BIT_MONO){
                pub_republish = it->advertiseCamera(_republishPubName, 1);
            }

                if (!cameraAdvertised) {
                        pub_8bit = it->advertiseCamera(_8bitPubName, 1);

                        cameraAdvertised = true;
                } else {
                        pub_8bit_im = it->advertise(_8bitPubName, 1);

                }

        }


        if (configData.outputColorFlag) {
            //HGH
            if (configData.republishSource==REPUBLISH_CODE_8BIT_COL){
                pub_republish = it->advertiseCamera(_republishPubName, 1);
            }
                if (!cameraAdvertised) {
                        pub_color = it->advertiseCamera(colorPubName, 1);

                        cameraAdvertised = true;
                } else {
                        pub_color_im = it->advertise(colorPubName, 1);

                }
        }

}


void streamerNode::handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
        
        if (configData.syncMode != SYNCMODE_HARD) {
                return;
        }
        
        if ((!configData.subscribeMode) && (!configData.resampleMode)) {
                return;
        }
        
        
        //ROS_ERROR("Handling an image...");
        
        //cout << "Handling camera..." << msg_ptr->header.stamp.toNSec() << ", " << image_time.toNSec() << endl;
        
        original_time = info_msg->header.stamp;
        original_bx = 0;
        original_by = 0;
        
        if (configData.inputDatatype == DATATYPE_DEPTH) {
                cv_ptr = cv_bridge::toCvCopy(msg_ptr, "16UC1");
        } else {
                cv_ptr = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);                                       // For some reason it reads as BGR, not gray
        }
        
        
        act_on_image();
        
}

void streamerNode::act_on_image() {
        
        //cout << "Acting on image..." << endl;
        
        updateCameraInfo();
        
        newImage = Mat(cv_ptr->image);
        
        Mat grayImage;
        
        if (newImage.type() == CV_16UC3) {
                cvtColor(newImage, frame, CV_RGB2GRAY);
        } else {
                frame = Mat(newImage);
        }
        
        if (readyToPublish) {
                // This means it still hasn't published the last requested frame
                //return;
        } else {
                readyToPublish = true;
        }
        
        
        if (!configData.resampleMode) {
                
                if (processImage()) {
                        if (readyToPublish) {
                                updateCameraInfo();
                                publishTopics();
                                writeData();
                        }
                }
                
        }
        
        return;
        
}

void streamerNode::handle_image(const sensor_msgs::ImageConstPtr& msg_ptr) {
        
        if (configData.syncMode == SYNCMODE_HARD) {
                return;
        }
        
        if ((!configData.subscribeMode) && (!configData.resampleMode)) {
                return;
        }

        //cout << "Handling image..." << msg_ptr->header.stamp.toNSec() << ", " << image_time.toNSec() << endl;
        
        original_time = msg_ptr->header.stamp;
        
        image_time = ros::Time::now();
        cv_ptr = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);                                       // For some reason it reads as BGR, not gray
        
        if (configData.syncMode == SYNCMODE_SOFT) {
                if (abs(image_time.toNSec() - info_time.toNSec()) < configData.soft_diff_limit) {
                        // ROS_ERROR("handle_image : image_time == info_time");
                        
                        image_time = dodgeTime;
                        act_on_image();
                }
        } else {
                act_on_image();
        }
        
        
        //ROS_ERROR("image header = (%d)", msg_ptr->header.stamp.toNSec());
                
        

        
}

streamerData::streamerData() {
        
        intrinsicsProvided = false;
        //wantsToRectify = false;
        //HGH
        wantsToAddExtrinsics = false;
        wantsToWrite = false;
        wantsToEncode = false;
        wantsToKeepNames = false;
        wantsToUndistort = false;
        wantsToResize = false;
        loopFlag = false;
        imageDimensionsSpecified = false;
        
        mapCode = CIECOMP;
        mapParam = 0;
        
        framerate = -1.0;
        
        soft_diff_limit = 5000000;
        
}

bool streamerData::obtainStartingData(ros::NodeHandle& nh) {
        
        nh.param<std::string>("source", source, "dev");
        
        nh.param<std::string>("file", filename, "file");
        nh.param<std::string>("dev", capture_device, "/dev/video0");
        nh.param<std::string>("topic", topicname, "/thermalvis/streamer/image_raw");
        nh.param<std::string>("folder", folder, "folder");
        
        nh.param<double>("syncDiff", syncDiff, 0.005);
        
        nh.param<std::string>("portAddress", portAddress, "/dev/ttyS0");
        
        nh.param<double>("alpha", alpha, 0.00);
        nh.param<bool>("autoAlpha", autoAlpha, true);
        
        nh.param<bool>("verboseMode", verboseMode, false);

        nh.param<int>("map", map, 0);
        nh.param<bool>("extremes", extremes, true);
        nh.param<std::string>("intrinsics", intrinsics, "intrinsics");
        
        nh.param<int>("inputWidth", inputWidth, 0);
        nh.param<int>("inputHeight", inputHeight, 0);
        
        
        nh.param<std::string>("extrinsics", extrinsics, "extrinsics");
        
        nh.param<int>("inputDatatype", inputDatatype, 1);
        
    nh.param<int>("camera_number", camera_number, 0);
        nh.param<double>("framerate", framerate, -1.0);
        
        nh.param<double>("writeQuality", writeQuality, 1.0);
        
        nh.param<bool>("output16bit", output16bitFlag, true);
        nh.param<bool>("output8bit", output8bitFlag, false);
        nh.param<bool>("outputColor", outputColorFlag, true);
        
        nh.param<bool>("loopMode", loopFlag, false);
        
        nh.param<bool>("disableSkimming", disableSkimming, true);
        
        nh.param<bool>("writeImages", wantsToWrite, false);
        nh.param<std::string>("outputFolder", outputFolder, "outputFolder");
        
        nh.param<bool>("serialComms", serialComms, false);
        
        nh.param<int>("calibrationMode", calibrationMode, CALIBMODE_OFF);
        
        if (outputFolder.size() > 0) {
                if (outputFolder.at(outputFolder.size()-1) == '/') {
                        outputFolder = outputFolder.substr(0, outputFolder.size()-1);
                }
        } else {
                ROS_WARN("Specified output folder is 0 characters long..");
        }
        
        
        if ((wantsToWrite) && (outputFolder.size() > 0)) {
                // Create necessary folders
                char folderCommand[256];
                sprintf(folderCommand, "mkdir -p %s", outputFolder.c_str());
                
                int res = system(folderCommand);
                
                if (res == 0) {
                        ROS_WARN("system() call returned 0...");
                }
        }
        
        
        ROS_INFO("outputFolder = (%s)", outputFolder.c_str());
        
        nh.param<int>("outputFormat", outputFormat, 4);
        nh.param<std::string>("outputType", outputType, "outputType");
        nh.param<bool>("keepOriginalNames", wantsToKeepNames, false);
        
        nh.param<bool>("writeVideo", wantsToEncode, false);
        nh.param<std::string>("outputVideo", outputVideo, "outputVideo");
        nh.param<std::string>("videoType", videoType, "videoType");
                
        nh.param<bool>("undistortImages", wantsToUndistort, false);
        //HGH
        nh.param<bool>("rectifyImages", wantsToRectify, false);
        
        nh.param<bool>("removeDuplicates", wantsToRemoveDuplicates, false);
        
        if (outputFolder != "outputFolder") {
                nh.param<bool>("dumpTimestamps", wantsToDumpTimestamps, false);
        } else {
                wantsToDumpTimestamps = false;
        }
        
        nh.param<bool>("resizeMode", wantsToResize, false);
        
        //nh.param<int>("syncMode", syncMode, SYNCMODE_HARD);
        
        nh.param<int>("rows", desiredRows, -1);
        nh.param<int>("cols", desiredCols, -1);
        
        if (wantsToResize) {
                if ((desiredRows < 1) || (desiredRows > MAX_ROWS) || (desiredCols < 1) || (desiredCols > MAX_COLS)) {
                        ROS_ERROR("Resizing values (%d, %d) invalid.",desiredCols, desiredRows);
                        return false;
                        
                } else {
                        ROS_INFO("Resizing to (%d x %d)", desiredCols, desiredRows);
                }
        }
        
        if (wantsToUndistort) {
                ROS_INFO("Undistorting images...");
        }
        
        nh.param<int>("normMode", normMode, 0);
        nh.param<std::string>("normalizationMode", normalizationMode, "normalizationMode");
        
        nh.param<double>("normFactor", normFactor, 0.0);
        
        if (normalizationMode == "standard") {
                normMode = 0;
        } else if (normalizationMode == "low_contrast") {
                normMode = 1;
        }
        
        /*
        if (wantsToWrite) {
                ROS_INFO("Has chosen to write.");
                
                if (outputFolder == "outputFolder") {
                        ROS_ERROR("outputFolder incorrectly specified...");
                        return false;
                } else {
                        ROS_INFO("outputFolder = (%s)", outputFolder.c_str());
                        
                }
                
                ROS_INFO("Image format = (%d); image type = (%s)", outputFormat, outputType.c_str());
                
                if (wantsToKeepNames) {
                        ROS_INFO("(retaining original names)");
                } else {
                        ROS_INFO("(not retaining original names)");
                }
                
        }
        */
        
        if (wantsToEncode) {
                ROS_INFO("Has chosen to encode.");
                
                if (outputVideo == "outputVideo") {
                        ROS_ERROR("outputVideo incorrectly specified...");
                        return false;
                } else {
                        ROS_INFO("outputVideo = (%s)", outputVideo.c_str());
                        
                }
                
                ROS_INFO("Image format = (%s); image type = (%s)", "avi", videoType.c_str());
                
        }
        
        if (inputDatatype == DATATYPE_8BIT) {
                ROS_INFO("Streaming mode: 8-bit");
        } else if (inputDatatype == DATATYPE_RAW) {
                ROS_INFO("Streaming mode: 16-bit");
        } else if (inputDatatype == DATATYPE_MM) {
                ROS_INFO("Streaming mode: multimodal");
        }

        // ROS_INFO("Source = %s", source.c_str());
        
        readMode = false;
        loadMode = false;
        captureMode = false;
        pollMode = false;
        subscribeMode = false;
        resampleMode = false;
        
        
        if (source == "dev") {
                
                if ((framerate < 0) || (framerate > MAX_READ_RATE)) {
                        ROS_INFO("Framerate set to (%f) so therefore defaulting to capture mode.", framerate);
                        framerate = MAX_READ_RATE;
                        captureMode = true;
                } else {
                        ROS_INFO("Requested framerate (%f) - polling mode", framerate);
                        pollMode = true;
                }
                
                ROS_INFO("Reading from a device (%s)", capture_device.c_str());
                
        } else if (source == "file") {
                readMode = true;
                
                if ((framerate < 0.0) || (framerate > MAX_READ_RATE)) {
                        ROS_INFO("Invalid framerate (%f) so defaulting to (%f).", framerate, DEFAULT_READ_RATE);
                        framerate = DEFAULT_READ_RATE;
                } else {
                        ROS_INFO("Requested framerate = %f", framerate);
                }
                
                ROS_INFO("Reading from a file (%s)", filename.c_str());
        } else if (source == "folder") {
                
                loadMode = true;
                
                if ((framerate < 0.0) || (framerate > MAX_READ_RATE)) {
                        ROS_WARN("Invalid framerate (%f) so defaulting to (%f).", framerate, DEFAULT_READ_RATE);
                        framerate = DEFAULT_READ_RATE;
                } else {
                        ROS_INFO("Requested framerate = %f", framerate);
                }
                
                ROS_INFO("Loading images from a folder (%s)", folder.c_str());
                
                
        } else if (source == "topic") {
                
                ROS_INFO("Subscribing to topic (%s) ", topicname.c_str());
                
                if ((framerate < 0.0) || (framerate > MAX_READ_RATE)) {
                        ROS_WARN("Invalid framerate (%f) so defaulting to subscribe mode.", framerate);
                        framerate = DEFAULT_READ_RATE;
                        subscribeMode = true;
                } else {
                        ROS_INFO("Requested framerate = %f (resample mode)", framerate);
                        resampleMode = true;
                }
                
        }
        
        
        //maxIntensityChange = 100 / DEFAULT_READ_RATE;
        
        if (loopFlag == true) {
                ROS_INFO("Option to loop has been selected.");
        }
        
        if (intrinsics != "intrinsics") {
                intrinsicsProvided = true;
                ROS_INFO("Intrinsics at (%s) selected.", intrinsics.c_str());
                
                if ((inputWidth != 0) && (inputHeight != 0)) {
                        ROS_WARN("Provided image dimensions will be ignored because of provided intrinsics file.");
                }
        } else {
                
                if ((inputWidth != 0) && (inputHeight != 0)) {
                        ROS_INFO("Provided image dimensions (%d, %d) will be used.", inputWidth, inputHeight);
                        imageDimensionsSpecified = true;
                } else {
                        ROS_WARN("No intrinsics or image dimensions provided. Will attempt to estimate camera size...");        
                }
                
                        
                //intrinsics = read_addr + "data/calibration/csiro-aslab/miricle-1.yml";
                //ROS_ERROR("No intrinsics supplied. Defaulting to (%s)", intrinsics.c_str());
        }
        
        
        
        if (extrinsics != "extrinsics") {
                ROS_WARN("Extrinsics at %s selected.", extrinsics.c_str());

                //HGH
                //wantsToRectify = false;

                //HGH
                wantsToAddExtrinsics = true;
                if (camera_number < 0) {
                        ROS_ERROR("Invalid camera number selected (%d) so defaulting to (0).", camera_number);
                        camera_number = 0;
                }else{
                        ROS_INFO("Camera number (%d).", camera_number);
                }
        }

        getMapping(map, extremes, mapCode, mapParam);
                
        int modeCount = 0;
        
        if (captureMode) modeCount++;
        if (pollMode) modeCount++;
        if (readMode) modeCount++;
        if (loadMode) modeCount++;
        if (subscribeMode) modeCount++;
        if (resampleMode) modeCount++;
        
        if (modeCount == 0) {
                ROS_ERROR("Either a device, file or topic should be specified for streaming.");
                return false;
        } else if (modeCount > 1) {
                ROS_ERROR("Either a device, file or topic should be specified - not more than one.");
                return false;
        }
        
        if ((framerate < -1.0) || (framerate > MAX_READ_RATE)) {
                framerate = DEFAULT_READ_RATE;
        }
        
        // ROS_INFO("normalization mode = (%d)", normMode);

       //HGH
        nh.param<int>("republishSource", republishSource, NO_REPUBLISH_CODE);
        nh.param<std::string>("republishTopic", republishTopic, "specifyTopic/image_raw");
        //check for valid republishSource
        switch(republishSource){
        case REPUBLISH_CODE_8BIT_MONO:
             ROS_WARN("Republishing mono image as %s", republishTopic.c_str() );
            break;
        case REPUBLISH_CODE_8BIT_COL:
            ROS_WARN("Republishing color image as %s", republishTopic.c_str() );
            break;
        case REPUBLISH_CODE_16BIT:
            ROS_WARN("Republishing 16bit image as %s", republishTopic.c_str() );
            break;
        default:
            republishSource = NO_REPUBLISH_CODE;
            break;
        }
        if (republishSource != NO_REPUBLISH_CODE){
            ROS_INFO("Republish Code: %d", republishSource);
        }

        nh.param<bool>("republishNewTimeStamp",republishNewTimeStamp,false);

        nh.param<std::string>("frameID", frameID, ""); //specify the frameID

        nh.param<bool>("drawReticle",drawReticle, false);


        
        return true;
}

void getMapping(int mapIndex, bool extremes, int& mapCode, int& mapParam) {
        
        if (mapIndex == 0) {
                mapCode = GRAYSCALE;
        } else if (mapIndex == 1) {
                mapCode = CIECOMP;
        } else if (mapIndex == 2) {
                mapCode = BLACKBODY;
        } else if (mapIndex == 3) {
                mapCode = RAINBOW;
        } else if (mapIndex == 4) {
                mapCode = IRON;
        } else if (mapIndex == 5) {
                mapCode = BLUERED;
        } else if (mapIndex == 6) {
                mapCode = JET;
        } else if (mapIndex == 7) {
                mapCode = ICE;
        } else if (mapIndex == 8) {
                mapCode = ICEIRON;
        } else if (mapIndex == 9) {
                mapCode = ICEFIRE;
        } else if (mapIndex == 10) {
                mapCode = REPEATED;
        } else if (mapIndex == 11) {
                mapCode = HIGHLIGHTED;
        } else {
                mapCode = GRAYSCALE;
        }
        
        if (extremes) {
                mapParam = 0;
        } else {
                mapParam = 1;
        }
        
}

bool streamerNode::isVideoValid() {
        
        if (::wantsToShutdown) {
                if (configData.verboseMode){ ROS_WARN("Wants to shut down.."); }
                setValidity(false);
        }       
                
        return videoValid;
        
}

void streamerNode::setValidity(bool val) {
        if ((val == false) && (configData.verboseMode)) { ROS_WARN("Validity being set to false.."); }
        videoValid = val;
}

streamerSource * streamerNode::getMainVideoSource() {
        return mainVideoSource;
}

VideoCapture * streamerNode::getVideoCapture() {
        return &cap;
}

void streamerNode::updateMap() {
        
        ROS_WARN("Updating map...");
        
        if (configData.autoAlpha) {
                configData.alpha = findBestAlpha(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.cameraSize);
                
                ROS_INFO("Optimal alpha: (%f)", configData.alpha);

        }
        
        Rect* validPixROI = 0;
        globalCameraInfo.newCamMat = getOptimalNewCameraMatrix(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.cameraSize, configData.alpha, globalCameraInfo.cameraSize, validPixROI, centerPrincipalPoint);
        
        //HGH initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.R, globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
        //HGH
        if (configData.wantsToAddExtrinsics){

            //update camera info extrinsics...
            getRectification();
            updateCameraInfoExtrinsics();

            if (configData.wantsToRectify){
                if (configData.camera_number == 0){
                    initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalExtrinsicsData.R0, globalExtrinsicsData.P0, globalCameraInfo.cameraSize, CV_32FC1,  map1, map2);
                }else if (configData.camera_number == 1){
                    initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalExtrinsicsData.R1, globalExtrinsicsData.P1, globalCameraInfo.cameraSize, CV_32FC1,  map1, map2);
                }
            }else{
                initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs,  Mat(), globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
            }

        }else{
            initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs,  Mat() , globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
        }

        alphaChanged = false;
        
        ROS_WARN("Map updated.");
}

void streamerNode::serverCallback(thermalvis::streamerConfig &config, uint32_t level) {
        
        if (configData.verboseMode){ ROS_INFO("Processing reconfigure request..."); }
        
        /*
        if (firstCall) {
                
                ROS_INFO("Initial Argument Summary:");
                
                ROS_INFO("outputFolder = (%s)", configData.outputFolder.c_str());
                
                ROS_INFO("config.inputDatatype = (%d)", config.inputDatatype);
                ROS_INFO("config.writeImages = (%s)", config.writeImages ? "true" : "false" );
                ROS_INFO("config.outputFormat = (%d)", config.outputFormat);
                ROS_INFO("config.outputType = (%d)", config.outputType);
                ROS_INFO("config.loopMode = (%s)", config.loopMode ? "true" : "false" );
                ROS_INFO("config.undistortImages = (%s)", config.undistortImages ? "true" : "false" );

                //HGH
                ROS_INFO("config.rectifyImages = (%s)", config.rectifyImages ? "true" : "false" );

                ROS_INFO("config.alpha = (%f)", config.alpha);
                ROS_INFO("config.removeDuplicates = (%s)", config.removeDuplicates ? "true" : "false" );
                
                firstCall = false;
        }
        */
        
        if (configData.detectorMode != config.detectorMode) {
                
                configData.detectorMode = config.detectorMode;
                
                if (!configData.serialComms) {
                        ROS_WARN("Selecting a detector mode has no effect unless serial comms are enabled.");
                } else {
                        updateDetectorMode = true;
                }
                
                
        }
        
        if (configData.readThermistor != config.readThermistor) {
                configData.readThermistor = config.readThermistor;
                
                if (configData.readThermistor) {
                        ROS_INFO("Wants to read from thermistor");
                        //ROS_WARN("Writing thermistor values to (%s)...", thermistorLogFile.c_str());
                        //ofs_thermistor_log.open(thermistorLogFile.c_str());
                } else {
                        //if (ofs_thermistor_log.is_open()) ofs_thermistor_log.close();
                }
                
        }
        
        configData.wantsToMarkDuplicates = config.markDuplicates;
        configData.wantsToOutputDuplicates = config.dumpDuplicates;
        
        configData.temporalSmoothing = config.temporalSmoothing;
        
        configData.maxThermistorDiff = config.maxThermistorDiff;
        
        configData.alternatePeriod = config.alternatePeriod;
        
        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));
                }
        }
        
        if (configData.maxNucInterval != config.maxNucInterval) {
                
                configData.maxNucInterval = config.maxNucInterval;
                updateNucInterval = true;
        }
        
        if (configData.maxNucThreshold != config.maxNucThreshold) {
                
                configData.maxNucThreshold = config.maxNucThreshold;
                updateNucInterval = true;
        }
        
        
        if (!configData.temporalSmoothing) {
                lastMedian = -1.0;
        }
        
        configData.verboseMode = config.verboseMode;
        
        configData.maxIntensityChange = config.maxShift;
        configData.maxPastMeans = config.temporalMemory;
            
        configData.syncDiff = config.syncDiff;
        
        configData.soft_diff_limit = (unsigned long) (configData.syncDiff * 1000000000.0);
        
        configData.loopFlag = config.loopMode; 
        
        configData.syncMode = config.syncMode;
        
        configData.normFactor = config.normFactor;

        bool updateWriteParams = false;
        if ((configData.writeQuality != config.writeQuality) || (configData.outputFormat != config.outputFormat)) {
                updateWriteParams = true;
        }
        
        if (configData.forceInputGray != config.forceInputGray) {
                firstFrame = true;
        }
        
        configData.forceInputGray = config.forceInputGray;
        
        configData.fixDudPixels = config.fixDudPixels;
        
        configData.writeQuality = config.writeQuality;
        
        if (config.framerate != 0.0) {
                configData.pauseMode = false;
        }
        
        if ((config.autoAlpha != configData.autoAlpha) || ((!config.autoAlpha) && (config.alpha != configData.alpha))) {
  
                configData.autoAlpha = config.autoAlpha;
                configData.alpha = config.alpha;
                
                alphaChanged = true;
                
                //ROS_WARN("Updating map...");
                updateMap();
                
                /*
                Rect* validPixROI = 0;
                globalCameraInfo.newCamMat = getOptimalNewCameraMatrix(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.cameraSize, configData.alpha, globalCameraInfo.cameraSize, validPixROI, centerPrincipalPoint);
                
                initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.R, globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
                */
                
        } else {

                //HGH
                if (configData.alpha != config.alpha){
                    if (configData.wantsToAddExtrinsics){
                        getRectification();
                        updateCameraInfoExtrinsics();
                    }
                }

                configData.alpha = config.alpha;
        }
        
        fusionFactor = config.fusionFactor;
        
        //ROS_WARN("Progressing...");
            
        if (config.framerate < 0.0) {
                
                if (configData.pollMode) {
                
                        ROS_WARN("Invalid framerate (%f) so switching to capture mode.", config.framerate);
                        configData.framerate = DEFAULT_READ_RATE;
                        configData.pollMode = false;
                        configData.captureMode = true;
                        
                } else if (configData.readMode) {
                        
                        ROS_WARN("Invalid framerate (%f) so defaulting to (%f).", config.framerate, DEFAULT_READ_RATE);
                        configData.framerate = DEFAULT_READ_RATE;

                } else if (configData.loadMode) {
                        
                        ROS_WARN("Invalid framerate (%f) so defaulting to (%f).", config.framerate, DEFAULT_READ_RATE);
                        configData.framerate = DEFAULT_READ_RATE;
                        
                } else if (configData.resampleMode) {
                        
                        ROS_WARN("Invalid framerate (%f) so switching to subscribe mode.", config.framerate);
                        configData.framerate = DEFAULT_READ_RATE;
                        configData.resampleMode = false;
                        configData.subscribeMode = true;
                        
                }

                timer.setPeriod(ros::Duration(1.0 / DEFAULT_READ_RATE));
                
        } else {
                
                if (configData.captureMode) {
                
                        ROS_WARN("Specified framerate (%f) so switching to poll mode.", config.framerate);
                        configData.framerate = config.framerate;
                        configData.captureMode = false;
                        configData.pollMode = true;
                        
                } else if (configData.subscribeMode) {
                        
                        ROS_WARN("Specified framerate (%f) so switching to resample mode.", config.framerate);
                        configData.framerate = config.framerate;
                        configData.subscribeMode = false;
                        configData.resampleMode = true;
                        
                }
                
                if (config.framerate > 0.0) {
                        timer.setPeriod(ros::Duration(1.0 / config.framerate));
                } else {
                        configData.pauseMode = true;
                        timer.setPeriod(ros::Duration(1.0 / 1.0));
                }
                
                
        }
        
        if (config.inputDatatype != configData.inputDatatype) {
                if (configData.readMode) {
                        ROS_WARN("You cannot change the bit-reading mode mid-stream. Consider restarting.");
                } else {
                        ROS_WARN("about to release device...");
                        releaseDevice();
                        ROS_WARN("device released...");
                        configData.inputDatatype = config.inputDatatype;
                        ROS_WARN("Changing to (%d)", configData.inputDatatype);
                        setupDevice();
                }

        }
        
        configData.filterMode = config.filterMode;
        configData.filterParam = config.filterParam;
        
        configData.maxReadAttempts = config.maxReadAttempts;
        
        configData.normMode = config.normMode;
        
        bool wantsToRefreshCameras = false;
        
        if (config.output16bit && !configData.output16bitFlag) {
                configData.output16bitFlag = true;
                wantsToRefreshCameras = true;
        } else if (!config.output16bit && configData.output16bitFlag) {
                configData.output16bitFlag = false;
                wantsToRefreshCameras = true;
        }
        
        if (config.output8bit && !configData.output8bitFlag) {
                configData.output8bitFlag = true;
                wantsToRefreshCameras = true;
        } else if (!config.output8bit && configData.output8bitFlag){
                configData.output8bitFlag = false;
                wantsToRefreshCameras = true;
        }
        
        if (config.outputColor && !configData.outputColorFlag) {
                configData.outputColorFlag = true;
                wantsToRefreshCameras = true;
        } else if (!config.outputColor && configData.outputColorFlag) {
                configData.outputColorFlag = false;
                wantsToRefreshCameras = true;
        }
        
        if (wantsToRefreshCameras) {
                refreshCameraAdvertisements();
        }
            
        getMapping(config.map, config.extremes, configData.mapCode, configData.mapParam);
        colourMap.load_standard(configData.mapCode, configData.mapParam);
    
        if ((configData.outputFolder != "outputFolder") && (configData.outputFolder != "")) {
                
                if (config.writeImages) {
                        
                        if (configData.wantsToWrite != config.writeImages) {
                        
                                // Create folders 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...");
                                        }
                                }
                                
                        }
                        
                }
                
                configData.wantsToWrite = config.writeImages;
                
                if (configData.wantsToDumpTimestamps != config.dumpTimestamps) {
                        
                        if (configData.wantsToDumpTimestamps) {
                                ofs.open(configData.outputTimeFile.c_str(), fstream::ate);
                        } else {
                                ofs.close();
                        }
                        
                }
                        
                configData.wantsToDumpTimestamps = config.dumpTimestamps;
                
        } else {
                ROS_WARN("No valid output folder specified...");
                configData.wantsToWrite = false;
                configData.wantsToDumpTimestamps = false;
        }
        
       //HGH
        configData.wantsToRectify = config.rectifyImages;

        if (config.undistortImages) {
                //if (map1.rows == 0) {
                    //HGH  initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.R, globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
                    //HGH
                    if (configData.wantsToAddExtrinsics){
                        if (config.rectifyImages){
                            if (configData.camera_number == 0){
                                initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalExtrinsicsData.R0, globalExtrinsicsData.P0, globalCameraInfo.cameraSize, CV_32FC1,  map1, map2);
                            }else if (configData.camera_number == 1){
                                initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalExtrinsicsData.R1, globalExtrinsicsData.P1, globalCameraInfo.cameraSize, CV_32FC1,  map1, map2);
                            }
                            //initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs, globalCameraInfo.R, globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_16SC2/*CV_32FC1*/, map1, map2);
                        }else{
                            initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs,  Mat(), globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
                        }
                    }else{
                        initUndistortRectifyMap(globalCameraInfo.cameraMatrix, globalCameraInfo.distCoeffs,  Mat(), globalCameraInfo.newCamMat, globalCameraInfo.cameraSize, CV_32FC1, map1, map2);
                    }
                  //  }
        }       

        configData.wantsToUndistort = config.undistortImages;
        
        configData.wantsToRemoveDuplicates = config.removeDuplicates;
        
        configData.outputFormat = config.outputFormat;
        
        //ROS_ERROR("configData.outputFormat = %d", configData.outputFormat);
        
        if (configData.outputFormat == 0) {
                configData.outputFormatString = "jpg";
        } else if (configData.outputFormat == 1) {
                configData.outputFormatString = "pgm";
        } else if (configData.outputFormat == 2) {
                configData.outputFormatString = "bmp";
        } else if (configData.outputFormat == 3) {
                configData.outputFormatString = "ppm";
        } else if (configData.outputFormat == 4) {
                configData.outputFormatString = "png";
        }
        
        if (updateWriteParams) {
                configData.outputFileParams.clear();
                int val;
                if (configData.outputFormatString == "png") {
                        configData.outputFileParams.push_back(CV_IMWRITE_PNG_COMPRESSION);
                        val = int((1.0-configData.writeQuality) * 9.0);
                        configData.outputFileParams.push_back(val);
                } else if (configData.outputFormatString == "jpg") {
                        configData.outputFileParams.push_back(CV_IMWRITE_JPEG_QUALITY);
                        val = int(configData.writeQuality * 100.0);
                        configData.outputFileParams.push_back(val);
                }
                //ROS_INFO("Write quality: (%d)", val);
        }
        
        //ROS_ERROR("configData.outputFormatString = %s", configData.outputFormatString.c_str());
        
        if (config.outputType == 0) {
                configData.outputType = "CV_8UC1";
        } else if (config.outputType == 1) {
                configData.outputType = "CV_8UC3";
                if (configData.outputFormatString == "pgm") {
                        ROS_WARN("PGM cannot write as CV_8UC3...");
                }
        } else if (config.outputType == 2) {
                configData.outputType = "CV_16UC1";
                if ((configData.outputFormatString == "jpg") || (configData.outputFormatString == "bmp")) {
                        ROS_WARN("JPG/BMP cannot write as CV_16UC1...");
                }
        }
        
        //HGH
        if (configData.drawReticle != config.drawReticle){
            ROS_INFO("config.drawReticle = (%s)", config.drawReticle ? "true" : "false" );
        }
        configData.drawReticle = config.drawReticle;

        if (!firstServerCallbackProcessed) {
                firstServerCallbackProcessed = true;
        }
        
        if (configData.verboseMode) { ROS_INFO("Reconfigure request complete..."); }
}
        
int getMapIndex(string mapping) {
        
        int map;
        
        if (mapping == "GRAYSCALE") {
                map = 0;
        } else if (mapping == "CIECOMP") {
                map = 1;
        } else if (mapping == "BLACKBODY") {
                map = 2;
        } else if (mapping == "RAINBOW") {
                map = 3;
        } else if (mapping == "IRON") {
                map = 4;
        } else if (mapping == "BLUERED") {
                map = 5;
        } else if (mapping == "JET") {
                map = 6;
        } else if (mapping == "ICE") {
                map = 7;
        } else if (mapping == "ICEIRON") {
                map = 8;
        } else if (mapping == "ICEFIRE") {
                map = 9;
        } else if (mapping == "REPEATED") {
                map = 10;
        } else if (mapping == "HIGHLIGHTED") {
                map = 11;
        } else {
                map = 0;
        }
        
        return map;
}

int streamerNode::mygetch() {
  struct termios oldt,newt;
  int ch;
  tcgetattr( STDIN_FILENO, &oldt );
  newt = oldt;
  newt.c_lflag &= ~( ICANON | ECHO );
  tcsetattr( STDIN_FILENO, TCSANOW, &newt );
  ch = getchar();
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
  return ch;
}

int streamerNode::open_port() {
   int fd;                                   /* File descriptor for the port */


   fd = open(configData.portAddress.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
   if (fd == -1)
   {                                              /* Could not open the port */
     fprintf(stderr, "open_port: Unable to open /dev/ttyS1 - %s\n",
             strerror(errno));
   }
   return (fd);
}
 
bool streamerNode::configureSerialComms() {
        
        
        struct termios options;
   
        mainfd = open_port();

        fcntl(mainfd, F_SETFL, FNDELAY); /* Configure port reading */
                                     /* Get the current options for the port */
        tcgetattr(mainfd, &options);
        cfsetispeed(&options, B115200); /* Set the baud rates to 115200 */
        cfsetospeed(&options, B115200);
    
                                   /* Enable the receiver and set local mode */
        options.c_cflag |= (CLOCAL | CREAD);
        options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
        options.c_cflag |= CSTOPB;
        options.c_cflag &= ~CSIZE;
        options.c_cflag |=  CS8; /* Select 8 data bits */
        options.c_cflag &= ~CRTSCTS; /* Disable hardware flow control */  
 
                                 /* Enable data to be processed as raw input */
        options.c_lflag &= ~(ICANON | ECHO | ISIG);
       
                                        /* Set the new options for the port */
        tcsetattr(mainfd, TCSANOW, &options);
        
        // Ensure shutter is open...
        
        
        
        {
                sprintf(serialCommand, "open\r");
                
                int bytesToWrite = 0;

                while (serialCommand[bytesToWrite] != '\r') {
                        bytesToWrite++;
                }
                bytesToWrite++;
                
                int n;
                //char buff[256];
                
                ROS_INFO("SerialComms: Opening shutter with (%d) %s", bytesToWrite, serialCommand);
                n = write(mainfd, serialCommand, bytesToWrite);          
                if (n!=bytesToWrite) puts("write() failed!\n");
        }
        
        {
                // Trying to clear serial comms buffer...
                int n;
                char buff[256]; 
                n = read(mainfd, buff, 256);
                //ROS_WARN("Message received = %s\n", buff);
        }
        
        int statusNum;
        
        
        
        if (configData.disableSkimming) {
                // Check current SKIMACTIVE status
                sprintf(serialCommand, "SKIMACTIVE\r");
                
                int bytesToWrite = 0;

                while (serialCommand[bytesToWrite] != '\r') {
                        bytesToWrite++;
                }
                bytesToWrite++;
                
                int n;
                
                ROS_INFO("SerialComms: Verifying SKIMACTIVE status (%s) ...", serialCommand);
                n = write(mainfd, serialCommand, bytesToWrite);          
                if (n!=bytesToWrite) {
                        ROS_ERROR("write() failed!");
                } 
                
                ros::Duration(1.0).sleep();
                
                char buff[256]; 
                n = read(mainfd, buff, 256);
                //ROS_WARN("Message received = (%s)\n", buff);
                //printf("%s\n", buff);
                
                //istringstream ss_buff( &buff[103]);
                
                statusNum = atof(&buff[17]);
                ROS_INFO("SKIMACTIVE status = (%d)\n", statusNum);
                
        }
        
        ros::Duration(1.0).sleep();
                                        
        if (configData.disableSkimming) {
                sprintf(serialCommand, "SKIMACTIVE 0\r");
                
                int bytesToWrite = 0;

                while (serialCommand[bytesToWrite] != '\r') {
                        bytesToWrite++;
                }
                bytesToWrite++;
                
                int n;
                //char buff[256];
                
                ROS_INFO("SerialComms: Disabling mean-shifting with (%d) %s", bytesToWrite, serialCommand);
                n = write(mainfd, serialCommand, bytesToWrite);          
                if (n!=bytesToWrite) puts("write() failed!\n");
        }
        
        {
                sprintf(serialCommand, "SAVE\r");
                
                int bytesToWrite = 0;

                while (serialCommand[bytesToWrite] != '\r') {
                        bytesToWrite++;
                }
                bytesToWrite++;
                
                int n;
                //char buff[256];
                
                ROS_INFO("SerialComms: Saving current settings...");
                n = write(mainfd, serialCommand, bytesToWrite);          
                if (n!=bytesToWrite) puts("write() failed!\n");
        }
        
        if (statusNum == 1) {
                ROS_ERROR("Camera must now be powercycled: SKIMACTIVE was not originally off.");
                return false;
        } else {
                ROS_WARN("Assuming camera has been powercycled since SKIMACTIVE was saved as off.");
                return true;
        }
        
}


float streamerNode::getThermistorReading() {
        
        double retVal;
        
        int n;
        char buff[256];
        
        n = write(mainfd, "THERM\r", 6);          
        
        if (n != 6) {
                puts("write() failed!\n");
        }

        n = read(mainfd, buff, 256);
        
        istringstream ss_buff( &buff[103]);
        
        retVal = atof(&buff[103]);
        
        return retVal;
        
}

---

thermalvis
Author(s): Stephen Vidas
autogenerated on Tue Mar 5 12:25:24 2013