7 #include "spinnaker_configure.h" 8 #include <boost/archive/binary_oarchive.hpp> 9 #include <boost/filesystem.hpp> 11 #include "std_msgs/Float64.h" 12 #include "std_msgs/String.h" 14 #include <dynamic_reconfigure/server.h> 15 #include <spinnaker_sdk_camera_driver/spinnaker_camConfig.h> 17 #include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h" 26 #ifdef trigger_msgs_FOUND 27 #include <trigger_msgs/sync_trigger.h> 44 virtual void onInit();
49 void init_variables_register_to_ros();
51 void init_cameras(
bool);
52 void start_acquisition();
53 void end_acquisition();
54 void deinit_cameras();
55 void acquire_mat_images(
int);
57 void run_external_trig();
60 void publish_to_ros(
int,
char**,
float);
62 void read_parameters();
63 std::string todays_date();
66 void write_queue_to_disk(queue<ImagePtr>*,
int);
67 void acquire_images_to_queue(vector<queue<ImagePtr>>*);
71 void set_frame_rate(CameraPtr,
float);
73 void create_cam_directories();
74 void save_mat_frames(
int);
75 void save_binary_frames(
int);
76 void get_mat_images();
79 void dynamicReconfigureCallback(spinnaker_sdk_camera_driver::spinnaker_camConfig &config, uint32_t level);
85 vector<acquisition::Camera>
cams;
107 double grab_time_, save_time_,
toMat_time_, save_mat_time_, export_to_ROS_time_, achieved_time_;
148 #ifdef trigger_msgs_FOUND 150 uint32_t prev_imu_trigger_count_ = 0;
151 uint32_t latest_imu_trigger_count_;
153 void assignTimeStampCallback(
const trigger_msgs::sync_trigger::ConstPtr& msg);
155 uint32_t latest_imu_trigger_count_;
158 std::vector<std::queue<SyncInfo_>> sync_message_queue_vector_;
176 std::shared_ptr<image_transport::ImageTransport>
it_;
188 spinnaker_sdk_camera_driver::SpinnakerImageNames
mesg;
int region_of_interest_height_
bool first_image_received
vector< vector< double > > distortion_coeff_vec_
vector< vector< Mat > > mem_frames_
int region_of_interest_width_
vector< string > cam_ids_
std::shared_ptr< boost::thread > pubThread_
vector< string > cam_names_
int region_of_interest_y_offset_
bool SOFT_FRAME_RATE_CTRL_
vector< vector< double > > intrinsic_coeff_vec_
vector< bool > flip_vertical_vec_
vector< string > time_stamps_
dynamic_reconfigure::Server< spinnaker_sdk_camera_driver::spinnaker_camConfig > * dynamicReCfgServer_
vector< bool > flip_horizontal_vec_
vector< sensor_msgs::ImagePtr > img_msgs
spinnaker_sdk_camera_driver::SpinnakerImageNames mesg
vector< CameraPtr > pCams_
std::shared_ptr< image_transport::ImageTransport > it_
vector< vector< double > > rect_coeff_vec_
boost::mutex queue_mutex_
double target_grey_value_
vector< sensor_msgs::CameraInfoPtr > cam_info_msgs
bool region_of_interest_set_
vector< acquisition::Camera > cams
vector< ImagePtr > pResultImages_
vector< string > imageNames
vector< image_transport::CameraPublisher > camera_image_pubs
bool MASTER_TIMESTAMP_FOR_ALL_
void run(ClassLoader *loader)
int region_of_interest_x_offset_
vector< vector< double > > proj_coeff_vec_
uint64_t SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_
ros::Publisher acquisition_pub