VirtualCameraNodelet.cpp

Go to the documentation of this file.

#include "VirtualCameraNodelet.hpp"

namespace virtual_camera {

VirtualCameraParametersPtr VirtualCameraNodelet::handleStartupParameters() {
        ros::NodeHandle & nh = this->getPrivateNodeHandle();
        VirtualCameraParametersPtr ret(new VirtualCameraParameters());
        nh.getParam("approxDistance", ret->approxDistance);
        nh.getParam("viewportWidth", ret->viewportWidth);
        nh.getParam("viewportHeight", ret->viewportHeight);
        nh.getParam("horizontalFov", ret->horizontalFov);
        nh.getParam("verticalFov", ret->verticalFov);
        nh.getParam("frameRate", ret->frameRate);
        nh.getParam("posX", ret->posX);
        nh.getParam("posY", ret->posY);
        nh.getParam("posZ", ret->posZ);
        nh.getParam("pan", ret->pan);
        nh.getParam("tilt", ret->tilt);
        nh.getParam("parentFrameId", ret->parentFrameId);
        nh.getParam("encoding", ret->encoding);
        nh.getParam("model", ret->model);
        nh.getParam("synchronize", ret->synchronize);
        this->synchronizer = RealCameraSynchronizerPtr(
                        new RealCameraSynchronizer(ret->synchronize,
                        this->imageSyncMutex));
        NODELET_INFO("Parameters successfully loaded.");
        return ret;
}


void VirtualCameraNodelet::handleNewCameraParams(
                const VirtualCameraParametersPtr & params) {
        this->synchronizer->setSynchronization(params->synchronize);
        this->camera->updateVirtualCameraParameters(params);
}


void VirtualCameraNodelet::addRealCamerasFromParams() {
        ros::NodeHandle & nh = this->getPrivateNodeHandle();
        int i = 1;
        std::string iS = boost::lexical_cast<std::string>(i);
        while (nh.hasParam("realCameraTopic" + iS)) {
                AddRealCameraPtr nCam(new AddRealCamera());
                nh.getParam("realCameraTopic" + iS, nCam->topic);
                nh.param<std::string>("realCameraFrame" + iS, nCam->frameId, "");
                this->newCameraCallback(nCam);
                ++i;
                iS = boost::lexical_cast<std::string>(i);
        }
}


void VirtualCameraNodelet::registerCamerasFromWaitList() {
        std::list<RealCameraPtr>::iterator it = this->addWaitList.begin();
        while (it != this->addWaitList.end()) {
                bool can = this->transformListener->canTransform((*it)->getName(),
                                this->camera->getName(), ros::Time(0));
                if (can && (*it)->isReady()) {
                        this->camera->addRealCamera(*it);
                        ROS_INFO_STREAM((*it)->getName() << " - camera ready and added.");
                        this->addWaitList.erase(it++);
                }else {
                        ROS_INFO_STREAM((*it)->getName() << " - camera not ready yet.");
                        ++it;
                }
        }
}


std::string VirtualCameraNodelet::getNameFromTopic(const std::string & topic) {
        int start = topic.length()-1;
        for (; start >= 0; --start){
                if (topic[start] == '/') break;
        }
        return topic.substr(start+1, topic.length()-(start+1));
}


void VirtualCameraNodelet::loop(const ros::TimerEvent& event) {
        static double lastRate = camera->getFrameRate();
        static image_transport::Publisher pub = this->it->advertise(
                        this->name + "/image", 1);
        if (lastRate != camera->getFrameRate()){
                lastRate = camera->getFrameRate();
                this->timer.setPeriod(ros::Duration(1/lastRate));
        }
        this->registerCamerasFromWaitList();
        this->cameraInfoPublisher.publish(this->camera->getCameraInfo());
        boost::mutex::scoped_lock(*(this->imageSyncMutex));
        pub.publish(camera->generateImage());
}


void VirtualCameraNodelet::newCameraCallback(AddRealCameraConstPtr cam) {
        NODELET_INFO("Adding new camera");
        std::string  frameId;
        if(cam->frameId != "") {
                 frameId = cam->frameId;
        }else {
                 frameId = this->getNameFromTopic(cam->topic);
        }
        ROS_INFO_STREAM("DEBUG: " << this->nh.resolveName(cam->topic + "/camera_info"));
        RealCameraPtr nCam(new RealMonoCamera(frameId, this->transformListener));
        this->realCamInfoSubs.push_back(
                                this->nh.subscribe(this->nh.resolveName(cam->topic + "/camera_info"),
                                                        1, &RealCamera::setCameraInfo, nCam));
        this->synchronizer->addCamera(this->nh.resolveName(cam->topic + "/image"),
                        nCam, this->it);
        this->addWaitList.push_back(nCam);
}


void VirtualCameraNodelet::onInit() {
        NODELET_INFO("Initialization ahead");
        this->nh = this->getNodeHandle();
        this->imageSyncMutex = MutexPtr(new boost::mutex());
        this->it = boost::shared_ptr<image_transport::ImageTransport>(
                        new image_transport::ImageTransport(nh));
        this->getPrivateNodeHandle().getParam("name", this->name);
        this->transformListener = boost::shared_ptr<tf::TransformListener>(new tf::TransformListener());
        this->camera = VirtualCameraPtr(new GeneralVirtualCamera(this->name,
                        this->handleStartupParameters()));
        this->cameraInfoPublisher = nh.advertise<sensor_msgs::CameraInfo>(
                        nh.resolveName(this->name + "/camera_info"), 1000);
        this->virtCamParamsSubscriber = nh.subscribe(
                        nh.resolveName(this->name + "/input_params"),
                        1, &VirtualCameraNodelet::handleNewCameraParams, this);
        this->addCameraSubscriber = nh.subscribe(
                        nh.resolveName(this->name + "/add_camera"),
                        1000, &VirtualCameraNodelet::newCameraCallback, this);
        timer = nh.createTimer(ros::Rate(this->camera->getFrameRate()),
                        &VirtualCameraNodelet::loop, this, false);

        this->addRealCamerasFromParams();
        NODELET_INFO("Successful start");
}


}