00001 00002 00003 00004 00005 00006 00007 00008 00009 00010 00011 00012 00013 00014 00015 00016 00017 00018 00019 00020 00021 00022 00023 00024 00025 00026 00027 00028 00029 00030 00031 00032 00033 #ifndef STEREO_CAMERA_H 00034 #define STEREO_CAMERA_H 00035 00036 #include <avt_vimba_camera/avt_vimba_camera.h> 00037 #include <avt_vimba_camera/AvtVimbaCameraConfig.h> 00038 #include <avt_vimba_camera/AvtVimbaCameraStereoConfig.h> 00039 #include <avt_vimba_camera/avt_vimba_api.h> 00040 00041 #include <ros/ros.h> 00042 #include <sensor_msgs/Image.h> 00043 #include <sensor_msgs/CameraInfo.h> 00044 #include <camera_info_manager/camera_info_manager.h> 00045 #include <image_transport/image_transport.h> 00046 #include <dynamic_reconfigure/server.h> 00047 #include <std_msgs/Float64.h> 00048 00049 #include <diagnostic_updater/diagnostic_updater.h> 00050 #include <diagnostic_updater/publisher.h> 00051 00052 #include <boost/bind.hpp> 00053 #include <boost/thread/thread.hpp> 00054 #include <boost/asio.hpp> 00055 #include <boost/date_time/posix_time/posix_time.hpp> 00056 00057 #include <string> 00058 #include <boost/thread/mutex.hpp> 00059 00060 using namespace boost; 00061 00062 namespace avt_vimba_camera { 00063 class StereoCamera { 00064 public: 00065 StereoCamera(ros::NodeHandle nh, ros::NodeHandle nhp); 00066 ~StereoCamera(void); 00067 void run(); 00068 00069 private: 00070 AvtVimbaApi api_; 00071 AvtVimbaCamera left_cam_; 00072 AvtVimbaCamera right_cam_; 00073 00074 diagnostic_updater::Updater updater_; 00075 diagnostic_updater::TopicDiagnostic* pub_freq_; 00076 diagnostic_updater::FunctionDiagnosticTask* sync_check_; 00077 bool show_debug_prints_; 00078 00079 // Parameters 00080 std::string left_ip_; 00081 std::string right_ip_; 00082 std::string left_guid_; 00083 std::string right_guid_; 00084 std::string left_camera_info_url_; 00085 std::string right_camera_info_url_; 00086 00087 // Node handles 00088 ros::NodeHandle nh_; 00089 ros::NodeHandle nhp_; 00090 ros::NodeHandle left_nhp_; 00091 ros::NodeHandle right_nhp_; 00092 00093 // Image transport 00094 image_transport::ImageTransport it_; 00095 00096 // ROS Camera publisher 00097 image_transport::CameraPublisher left_pub_; 00098 image_transport::CameraPublisher right_pub_; 00099 00100 // Publish camera temperatures 00101 ros::Publisher pub_left_temp_; 00102 ros::Publisher pub_right_temp_; 00103 00104 boost::shared_ptr<camera_info_manager::CameraInfoManager> left_info_man_; 00105 boost::shared_ptr<camera_info_manager::CameraInfoManager> right_info_man_; 00106 00107 // Dynamic reconfigure 00108 typedef avt_vimba_camera::AvtVimbaCameraConfig Config; 00109 typedef avt_vimba_camera::AvtVimbaCameraStereoConfig StereoConfig; 00110 typedef dynamic_reconfigure::Server<StereoConfig> ReconfigureServer; 00111 ReconfigureServer reconfigure_server_; 00112 00113 // Camera configuration 00114 StereoConfig camera_config_; 00115 00116 // Check for errors 00117 double desired_freq_; 00118 bool left_init_; 00119 bool right_init_; 00120 double l_last_time_; 00121 double r_last_time_; 00122 00123 // Sync 00124 std::vector<sensor_msgs::Image> r_imgs_buffer_; 00125 std::vector<sensor_msgs::Image> l_imgs_buffer_; 00126 int imgs_buffer_size_; 00127 mutex l_sync_mutex_; 00128 mutex r_sync_mutex_; 00129 double max_sec_diff_; 00130 00131 // Check sync timer 00132 boost::asio::io_service io_; 00133 boost::asio::deadline_timer check_timer_; 00134 boost::asio::deadline_timer sync_timer_; 00135 double sync_timer_step_; 00136 00137 void leftFrameCallback(const FramePtr& vimba_frame_ptr); 00138 void rightFrameCallback(const FramePtr& vimba_frame_ptr); 00139 void syncCallback(); 00140 void configure(StereoConfig& newconfig, uint32_t level); 00141 void updateCameraInfo(const StereoConfig& config); 00142 void copyConfig(StereoConfig& sc, Config& lc, Config& rc); 00143 void checkCallback(); 00144 }; 00145 } 00146 #endif