System.h
Go to the documentation of this file.
00001 // -*- c++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 //
00004 // System.h
00005 //
00006 // Defines the System class
00007 //
00008 // This stores the main functional classes of the system, like the
00009 // mapmaker, map, tracker etc, and spawns the working threads.
00010 //
00011 #ifndef __SYSTEM_H
00012 #define __SYSTEM_H
00013 //#include "VideoSource.h"
00014 
00015 #include <ros/ros.h>
00016 #include <tf/tf.h>
00017 #include <tf/transform_broadcaster.h>
00018 #include <tf/transform_listener.h>
00019 #include <image_transport/image_transport.h>
00020 #include <sensor_msgs/image_encodings.h>
00021 #include <sensor_msgs/Imu.h>
00022 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00023 #include <ptam_com/PointCloud.h>
00024 #include <ptam_com/KeyFrame_srv.h>
00025 #include <std_msgs/String.h>
00026 #include <queue>
00027 
00028 #include "GLWindow2.h"
00029 //#include "ptam/RosNode.h"
00030 #include "ptam/Params.h"
00031 
00032 #include <cvd/image.h>
00033 #include <cvd/rgb.h>
00034 #include <cvd/byte.h>
00035 
00036 #include <TooN/TooN.h>
00037 #include <TooN/se3.h>
00038 #include <TooN/so3.h>
00039 
00040 class ATANCamera;
00041 struct Map;
00042 class MapMaker;
00043 class Tracker;
00044 class MapViewer;
00045 
00046 class System
00047 {
00048   typedef std::queue<sensor_msgs::Imu> ImuQueue;
00049   typedef std::queue<geometry_msgs::PoseWithCovarianceStamped> PoseQueue;
00050 
00051 public:
00052   System();
00053   void Run();
00054 
00055 private:
00056   ros::NodeHandle nh_, image_nh_;
00057   ros::Subscriber sub_imu_;
00058   ros::Subscriber sub_calibration_;
00059   ros::Subscriber sub_kb_input_;
00060   tf::TransformBroadcaster tf_pub_;
00061   tf::TransformListener tf_sub_;
00062   image_transport::Subscriber sub_image_;
00063   image_transport::Publisher pub_preview_image_;
00064   ros::Publisher pub_pose_;             // world in the camera frame
00065   ros::Publisher pub_pose_world_;       // camera in the world frame
00066   ros::Publisher pub_info_;
00067   ros::ServiceServer srvPC_;
00068   ros::ServiceServer srvKF_;
00069 
00070   ros::CallbackQueue image_queue_;
00071 
00072   ImuQueue imu_msgs_;
00073 
00074   bool first_frame_;
00075 
00076   GLWindow2 *mGLWindow;
00077   MapViewer *mpMapViewer;
00078 
00079   CVD::Image<CVD::byte > img_bw_;
00080   CVD::Image<CVD::Rgb<CVD::byte> > img_rgb_;
00081 
00082   Map *mpMap; 
00083   MapMaker *mpMapMaker; 
00084   Tracker *mpTracker; 
00085   ATANCamera *mpCamera;
00086 
00087 //  bool mbDone;
00088 
00089   void init(const CVD::ImageRef & size);
00090 
00091   void publishPoseAndInfo(const std_msgs::Header & header);
00092   void publishPreviewImage(CVD::Image<CVD::byte> & img, const std_msgs::Header & header);
00093   bool pointcloudservice(ptam_com::PointCloudRequest & req, ptam_com::PointCloudResponse & resp);
00094   bool keyframesservice(ptam_com::KeyFrame_srvRequest & req, ptam_com::KeyFrame_srvResponse & resp);
00095 
00096   void imageCallback(const sensor_msgs::ImageConstPtr & img);
00097   void imuCallback(const sensor_msgs::ImuConstPtr & msg);
00098   void keyboardCallback(const std_msgs::StringConstPtr & kb_input);
00099 
00100   bool transformQuaternion(const std::string & target_frame, const std_msgs::Header & header, const geometry_msgs::Quaternion & q_in, TooN::SO3<double> & r_out);
00101   bool transformPoint(const std::string & target_frame, const std_msgs::Header & header, const geometry_msgs::Point & t_in, TooN::Vector<3> & t_out);
00102   void quaternionToRotationMatrix(const geometry_msgs::Quaternion & q, TooN::SO3<double> & R);
00103 
00105   template<class T> bool findClosest(const ros::Time & timestamp, std::queue<T> & queue, T * obj, const double & max_delay = 0.01);
00106 
00107   static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams);
00108 
00109 };
00110 
00111 
00112 
00113 #endif


ptam
Author(s): Stephan Weiss, Markus Achtelik, Simon Lynen
autogenerated on Tue Jan 7 2014 11:12:22