$search

streamer.hpp

Go to the documentation of this file.

#ifndef _THERMALVIS_STREAMER_H_
#define _THERMALVIS_STREAMER_H_
        
#include "time.h"

#include <signal.h>

#include "ros_resources.hpp"
#include "tools.hpp"
#include "improc.hpp"
#include "video.hpp"

#include <dynamic_reconfigure/server.h>
#include "streamerConfig.h"

#include "clahe.h"

// INTERNAL TIMESTAMP STUFF
//#include "/usr/src/linux-headers-3.2.0-27/include/linux/unaligned/le_byteshift.h"
/*
#include <linux/kernel.h>
#include <linux/list.h>
#include <linux/usb.h>
#include <linux/videodev2.h>
#include <asm/unaligned.h>
*/
// SERIAL COMMS STUFF
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/signal.h>
#include <sys/types.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

#define BAUDRATE B38400
#define MODEMDEVICE "/dev/ttyS0"
#define _POSIX_SOURCE 1         //POSIX compliant source
#define FALSE 0
#define TRUE 1

volatile int STOP=FALSE;

void signal_handler_IO (int status);    //definition of signal handler
int wait_flag=TRUE;                     //TRUE while no signal received
char devicename[80];
long Baud_Rate = 115200;         // default Baud Rate (110 through 38400)
long BAUD;                      // derived baud rate from command line
long DATABITS;
long STOPBITS;
long PARITYON;
long PARITY;
int Data_Bits = 8;              // Number of data bits
int Stop_Bits = 1;              // Number of stop bits
int Parity = 0;                 // Parity as follows:
                  // 00 = NONE, 01 = Odd, 02 = Even, 03 = Mark, 04 = Space
int Format = 4;
FILE *input;
FILE *output;
int status;

char message[90];

// Other stuff from: http://ubuntuforums.org/showthread.php?t=1395180
#include <stdio.h>
#include <string.h>
#include <unistd.h> //UNIX Standard function definitions
#include <fcntl.h> //File control
#include <errno.h> //Error number def
#include <termios.h> //POSIX terminal control
#include <termio.h>

#define DEFAULT_SERIAL_POLLING_RATE 0.1

// END SERIAL COMMS STUFF

#define DEFAULT_READ_RATE 25.0
#define MAX_READ_RATE 100.0
#define FASTEST_READ_RATE 1000.0

#define DEFAULT_IMAGE_WIDTH 640
#define DEFAULT_IMAGE_HEIGHT 480

#define MAX_ROWS 1920
#define MAX_COLS 2560

#define DATATYPE_8BIT   0
#define DATATYPE_RAW    1
#define DATATYPE_MM     2
#define DATATYPE_DEPTH  3

#define UVC_STREAM_PTS                                  (1 << 2)
#define UVC_STREAM_SCR                                  (1 << 3)

#define NO_REPUBLISH_CODE 0
#define REPUBLISH_CODE_8BIT_MONO 1
#define REPUBLISH_CODE_8BIT_COL 2
#define REPUBLISH_CODE_16BIT 3

#define SYNCMODE_HARD                                           0
#define SYNCMODE_SOFT                                           1
#define SYNCMODE_IMAGEONLY                                      2

#define CALIBMODE_OFF                                           0
#define CALIBMODE_ALT_SHUTTER                           1 // Alternates shutter and sends NUCs
#define CALIBMODE_ALT_OUTPUT                            2 // Alternates between RAW and INS camera outputs
#define CALIBMODE_LONG_SHUTTER                          3 // Alternates shutter, but lets NUCs occur naturally

#define DETECTOR_MODE_RAW                                       0
#define DETECTOR_MODE_LUM                                       1
#define DETECTOR_MODE_INS                                       2
#define DETECTOR_MODE_RAD                                       3
#define DETECTOR_MODE_TMP                                       4

const char __PROGRAM__[] = "STREAMER";

typedef dynamic_reconfigure::Server < thermalvis::streamerConfig > Server;

bool wantsToShutdown = false;
void mySigintHandler(int sig);
int getMapIndex(string mapping);
void getMapping(int mapIndex, bool extremes, int& mapCode, int& mapParam);

struct camData_ {
        Mat cameraMatrix, distCoeffs, imageSize;
        
        Size cameraSize;
        Mat newCamMat;
        
        Mat R, T;
};

//HGH
struct camExtrinsicsData_ {
    Mat R, T;
    Mat R0, R1, T0, T1, P0, P1, cameraMatrix0, cameraMatrix1, distCoeffs0, distCoeffs1;
    Rect roi0, roi1;
};

struct streamerData {

        bool wantsToMarkDuplicates;
        bool wantsToOutputDuplicates;
        
        int calibrationMode; // various optional modes for calibration
        int alternatePeriod;
        
        int dummy;
        
        int maxReadAttempts;
        
        int inputWidth, inputHeight;
        bool imageDimensionsSpecified;
        
        double serialPollingRate;
        
        bool outputColorFlag;
        bool output16bitFlag;
        bool output8bitFlag;
        
        
        
        bool verboseMode;
        
        bool serialComms, readThermistor;
        string portAddress;
        
        int maxNucInterval;
        double maxNucThreshold;
        
        bool captureMode;
        bool readMode;
        bool loadMode;
        bool subscribeMode;
        bool resampleMode;
        bool pollMode;
        bool wantsToUndistort;
        
        bool forceInputGray;
        bool fixDudPixels;
        
        bool disableSkimming;
        
        bool loopFlag;
        bool wantsToResize;
        bool wantsToDumpTimestamps;
        bool wantsToRemoveDuplicates;
        
        bool temporalSmoothing;
        
        bool pauseMode;
        int syncMode;
        
        double syncDiff;
        
        bool extremes;
        
        double writeQuality;
        
        vector<int> outputFileParams;
        
        string read_addr;
        string source, filename, folder, capture_device, intrinsics, extrinsics, topicname;
        
        int map;
        
        string normalizationMode;
        double normFactor;
        int normMode;
        int inputDatatype;
        int detectorMode;
        int camera_number;
        int filterMode;
        double filterParam;
        
        double framerate;
        
        double maxThermistorDiff;
        
        unsigned long soft_diff_limit;
        
        bool obtainStartingData(ros::NodeHandle& nh);  
        
        int desiredRows;
        int desiredCols;
        
        string timeStampsAddress;
        
        int mapCode;
        int mapParam;
        
        double maxIntensityChange;
        int maxPastMeans;
        
        
        bool intrinsicsProvided, wantsToRectify, wantsToWrite, wantsToKeepNames, wantsToEncode;

        //HGH
        bool wantsToAddExtrinsics;
        string republishTopic;
        int republishSource;
        bool republishNewTimeStamp;
        bool drawReticle;
        
        int outputFormat;
        string outputFolder, outputFormatString, outputType, outputTimeFile;
        string outputVideo, videoType;
        
        double alpha;
        bool autoAlpha;

        string frameID;
        
        streamerData(); 
        
};

class streamerNode {
private:

        char nodeName[256];

        bool firstCall;
        
        bool updateNucInterval;
        bool updateDetectorMode;
        char serialCommand[256];
        
        bool altStatus;
        bool performingNuc;
        
        ofstream ofs;

        int alternateCounter;
        
        double pastMeans[256];
        int pastMeanIndex;
        
        string callLogFile, retrieveLogFile, internalLogFile, writeLogFile, duplicatesLogFile, thermistorLogFile;
        ofstream ofs_call_log, ofs_internal_log, ofs_retrieve_log, ofs_write_log, ofs_duplicates_log, ofs_thermistor_log;
        
        Mat testMat;
        
        double medianPercentile;
        
        float lastThermistorReading, newThermistorReading;
        
        image_transport::ImageTransport *it;
        streamerData configData;
        image_transport::Subscriber image_sub;
        ros::Subscriber info_sub;
        image_transport::CameraSubscriber camera_sub;
        
        ros::Time info_time, image_time, original_time, dodgeTime;
        
        double shiftDiff;
        
        unsigned long original_bx, original_by;

        
        unsigned long internal_time;
        
        int64_t firmwareTime;
        
        cv_bridge::CvImagePtr cv_ptr;
        sensor_msgs::Image msg_color, msg_16bit, msg_8bit;

        bool firstServerCallbackProcessed;
        
        double fusionFactor;
        bool centerPrincipalPoint;

        double lastMedian, newMedian, lastLowerLimit, lastUpperLimit, oldMaxDiff;

        double minVal, maxVal;
        
        Mat lastFrame;

        bool firstFrame;

        Mat map1, map2;
        
        Mat rzMat;

        cScheme colourMap;

        int writeIndex;
        int frameCounter;
        int lastWritten;
        
        VideoWriter vid_writer;
        
        bool videoInitialized;

        bool readyToPublish;

        ros::Timer timer, serial_timer;
        
        bool alphaChanged;
                
        image_transport::Publisher pub_color_im;
        image_transport::Publisher pub_16bit_im;
        image_transport::Publisher pub_8bit_im;
        //HGH
        image_transport::Publisher pub_republish_im;
        
        image_transport::CameraPublisher pub_color;
        image_transport::CameraPublisher pub_16bit;
        image_transport::CameraPublisher pub_8bit;
        //HGH
        image_transport::CameraPublisher pub_republish;

        Mat _8bitMat, _16bitMat, colourMat;
        Mat preFilteredMat, smoothedMat;
        Mat _8bitMat_pub, _16bitMat_pub, colourMat_pub;
        Mat newImage, normalizedMat, frame, workingFrame, undistorted;
        
        int device_num;

        camData_ globalCameraInfo;
        //HGH
        camExtrinsicsData_ globalExtrinsicsData;


        sensor_msgs::CameraInfo camera_info;
        
        ros::ServiceServer set_camera_info;

        bool isActuallyGray;

        
        
        vector<string> inputList;
        int fileCount;
        
        
        
        bool videoValid;
        streamerSource *mainVideoSource;
        VideoCapture cap;
        
        dynamic_reconfigure::Server<thermalvis::streamerConfig> server;
        dynamic_reconfigure::Server<thermalvis::streamerConfig>::CallbackType f;
        
        // Serial Comms
        int mainfd; /* File descriptor */


        
public:
        
        CvCapture* capture;
        
        bool isVideoValid();
        void setValidity(bool val);
        streamerSource * getMainVideoSource();
        VideoCapture * getVideoCapture();

        streamerNode(ros::NodeHandle& nh, streamerData startupData);
        
        void handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
        
        void handle_image(const sensor_msgs::ImageConstPtr& msg_ptr);
        
        void handle_info(const sensor_msgs::CameraInfoConstPtr& info_msg);
        
        bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp);
        
        void serialCallback(const ros::TimerEvent& e);
        void timerCallback(const ros::TimerEvent& e);
        bool streamCallback(bool capture = true);
        void acceptImage(void *ptr);
        bool processImage();
        void publishTopics();
        void writeData();
        bool processFolder();
        void updateCameraInfo();
        void assignCameraInfo();
        void overwriteCameraDims();
        void assignDefaultCameraInfo();

        //HGH
        void updateCameraInfoExtrinsics();

        void serverCallback(thermalvis::streamerConfig &config, uint32_t level);
        void refreshCameraAdvertisements();
        void initializeMessages();
        
        void act_on_image();
        
        // Source alternatives
        bool runBag();
        bool runRead();
        bool runLoad();
        bool runDevice();
        
        void updateMap();
        
        void prepareForTermination();
        
        void markCurrentFrameAsDuplicate();
        
        bool configureSerialComms();
        float getThermistorReading();
        
        bool setupDevice();
        void releaseDevice();
        
        int mygetch();
        int open_port();

        //HGH
        void getRectification();
        
};

boost::shared_ptr < streamerNode > *globalNodePtr;

#endif

---

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