RealCameraSynchronizer.cpp

Go to the documentation of this file.

#include "RealCameraSynchronizer.hpp"

namespace virtual_camera {


RealCameraSynchronizer::RealCameraSynchronizer(bool synchronize,
                const MutexPtr & syncMutex) {
        this->setSynchronization(synchronize);
        this->syncMutex = syncMutex;
        this->recentCount = 0;
        this->mostRecentStamp = ros::Time(0);
        this->cameraCount = 0;
}


void RealCameraSynchronizer::setSynchronization(bool mode) {
        this->synchronize = mode;
}


bool RealCameraSynchronizer::isSynchronized() {
        return this->synchronize;
}


void RealCameraSynchronizer::addCamera(const std::string & topic,
                const RealCameraPtr & camera,
                const boost::shared_ptr<image_transport::ImageTransport> & it) {
        boost::mutex::scoped_lock lock(*(this->syncMutex));
        this->realCamImgSubs.push_back(
                                it->subscribe(topic,
                                                        1, &RealCameraSynchronizer::generalCallback,
                                                        this));
        this->camsByFrame[camera->getName()] = camera;
        this->cameraCount++;
}


void RealCameraSynchronizer::generalCallback(
                const sensor_msgs::ImageConstPtr & image) {
        const std::string & camId = image->header.frame_id;
        if (this->synchronize) {
                this->handleSynchronization(image);
        } else {
                this->camsByFrame.find(camId)->second->updateImage(image);
        }
}


void RealCameraSynchronizer::handleSynchronization(
                const sensor_msgs::ImageConstPtr & image) {
        boost::mutex::scoped_lock lock(*(this->syncMutex));
        const ros::Time & stamp = image->header.stamp;
        this->msgsByFrame[image->header.frame_id] = image;
        if (this->mostRecentStamp > ros::Time::now()) {
                //because of bag files
                this->mostRecentStamp = ros::Time::now();
        }
        if (stamp > this->mostRecentStamp) {
                this->mostRecentStamp = stamp;
                this->recentCount = 1;
        }else if (stamp == this->mostRecentStamp) {
                this->recentCount++;
                if (this->recentCount == this->cameraCount) {
                        this->pushMessages();
                }
        }
}


void RealCameraSynchronizer::pushMessages() {
        std::map<std::string, sensor_msgs::ImageConstPtr>::iterator msgIt =
                        this->msgsByFrame.begin();
        std::map<std::string, RealCameraPtr>::iterator camIt =
                                this->camsByFrame.begin();
        //taking advantage of the ordering of the maps and the fact that
        //both have the same keys.
        while (msgIt != this->msgsByFrame.end()) {
                camIt->second->updateImage(msgIt->second);
                ++camIt;
                ++msgIt;
        }
}

}