CameraROS.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------




/* TODO:
 - subscribe /camera/camera_info for camera parameters (optional) 
 - subscribe /camera/image_raw for camera images (mandatory)
 - manage image acquisition "on demand"
 - ask Paolo about the new scheduler (camera may open a new thread to acquire images,
  it seems to be better than polling on camera for the last image)
 - convert images to opencv format
*/

#ifndef CAMERAROS_H
#define CAMERAROS_H

#ifdef MIP_HOST_APPLE
 #include <applePatch.h>
#endif

#include <Camera.h>

#include <includeROS.h>
#include <SpacesArma.h>
#include <CameraParameters.h>
#include <ROSNode.h>
//#include <sensor_msgs/Imu.h>

// Algorithm to extract feature from camera view
//#include <BearExtrCam.h>

namespace MipResources{
 /* @{ */
 
 
 class CameraRosOptions : public Options {
  public:
   StringOption  *infoCamTopicName;
   StringOption  *imageTopicName;
   StringOption  *cameraNode;
   StringOption  *nodeName;
   BoolOption   *addId;
   
   StringOption *robotName;
   
   CameraRosOptions();
   
   string getObjectName() const{
    return "CameraROS";
   }
   OptionGroupsType getGroup() {
    return OPT_GRP_RESO;
   }
 };
 
 class CameraROSPar{
  private:
   CameraPar3DArma*  _infoCam;
   
  public:
   
   CameraROSPar();
   
   CameraROSPar(const CameraROSPar& A);
   
   CameraROSPar& operator=(const CameraROSPar& A);
   
   CameraPar3DArma* getCamPars();
   
   void setCamPars(CameraPar3DArma* cPar);
   
   void initCamPars(const Decimal &info1, const Decimal &info2, const Decimal &info3,int opt, const Position2DArma &pp=Position2DArma(), const Decimal sk = 0.0, const Roto3DHomoArma &rcfw=Roto3DHomoArma(IDENTITY_ARMA_MATRIX),CanonicalCameraArmaMatrix cc=PIXEL_CAMERA_ARMA_MATRIX);
 };
 
 class CameraROS : public Camera {
  protected:
   ros::NodeHandle*       handleImage_subscriber;
   ros::Subscriber*        _subscriberCam;
   ros::Subscriber*        _subscriberInfoCam;
   
   bool              _firstParsInit;
   
   uint             _step;
   
//    CameraPar3DArma*       _pars;
   CameraROSPar*         _pars;
   
   CameraRosOptions*       _options;
   
   // CAMERA IMAGE
   vector<uint8_t>        _currImage;
   cv::Mat            _currCVImage;
   cv::Mat            _currCVModImage;
   ros::Time           _currImageTime;
   bool              _firstImageInit;
   
  public:
   CameraROS(int argc, const char** argv);
   
   CameraROS(int argc, char** argv,int opt);
   
   ~CameraROS();
   
   void cameraROSCallback(const sensor_msgs::Image::ConstPtr& msg);
   
   void cameraROSInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg);
   
   void updateCallbacks();
   
   ResourcePlate getPlate() const {
    return CAM_ROS_RES;
   }
   
   cv::Mat getImage();
   
   ros::Time getImageTime();
   
   void getImage(cv::Mat &image, ros::Time &time);
 };

}; // end of namespace

#endif // CAMERAROS_H

/* @} */