sync.cpp
Go to the documentation of this file.
1 
33 #include <avt_vimba_camera/sync.h>
34 #include <stdlib.h>
35 
36 namespace avt_vimba_camera {
37 
38 Sync::Sync(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), nhp_(nhp), init_(false), it_(nh)
39 {
40  // Read params
41  nhp_.param("camera", camera_, string("/stereo_down"));
42  nhp_.param("timer_period", timer_period_, 10.0);
43  nhp_.param("max_unsync_time", max_unsync_time_, 5.0);
44 }
45 
46 void Sync::run()
47 {
48  // Create the approximate sync subscriber
49  image_transport::SubscriberFilter left_sub, right_sub;
50  message_filters::Subscriber<sensor_msgs::CameraInfo> left_info_sub, right_info_sub;
51 
52  left_sub .subscribe(it_, camera_+"/left/image_raw", 5);
53  right_sub .subscribe(it_, camera_+"/right/image_raw", 5);
54  left_info_sub .subscribe(nh_, camera_+"/left/camera_info", 5);
55  right_info_sub.subscribe(nh_, camera_+"/right/camera_info", 5);
56 
58  sync_var.reset(new SyncType(SyncPolicy(5), left_sub, right_sub, left_info_sub, right_info_sub) );
59  sync_var->registerCallback(bind(&Sync::msgsCallback, this, _1, _2, _3, _4));
60 
61  // Sync timer
63 
64  // Publish info
65  pub_info_ = nhp_.advertise<std_msgs::String>("info", 1, true);
66 
67  ros::spin();
68 }
69 
70 void Sync::msgsCallback(const sensor_msgs::ImageConstPtr& l_img_msg,
71  const sensor_msgs::ImageConstPtr& r_img_msg,
72  const sensor_msgs::CameraInfoConstPtr& l_info_msg,
73  const sensor_msgs::CameraInfoConstPtr& r_info_msg)
74 {
75  if (!init_)
76  ROS_INFO("[SyncNode]: Initialized.");
77  init_ = true;
78 
80 }
81 
83 {
84  if (!init_) return;
85 
86  double now = ros::Time::now().toSec();
87 
88  // Check desired frequency
89  if (now - last_ros_sync_ > max_unsync_time_)
90  {
91  // No sync!
92  ROS_WARN_STREAM("[SyncNode]: No sync during " << now - last_ros_sync_ << " sec.");
93 
94  // Publish info
95  std_msgs::String msg;
96  msg.data = "Reseting camera driver at ROSTIME: " +
97  boost::lexical_cast<string>(now) + "s.";
98  pub_info_.publish(msg);
99  }
100 }
101 
102 
103 };
msg
void publish(const boost::shared_ptr< M > &message) const
image_transport::ImageTransport it_
Definition: sync.h:84
ros::Timer sync_timer_
Maximum time without sync
Definition: sync.h:73
ros::NodeHandle nhp_
Definition: sync.h:67
ROSCPP_DECL void spin(Spinner &spinner)
double timer_period_
Last ros time sync
Definition: sync.h:71
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > SyncPolicy
Camera name
Definition: sync.h:80
#define ROS_INFO(...)
Sync(ros::NodeHandle nh, ros::NodeHandle nhp)
Definition: sync.cpp:38
bool param(const std::string &param_name, T &param_val, const T &default_val) const
double last_ros_sync_
True when node is initialized.
Definition: sync.h:70
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
double max_unsync_time_
Timer period
Definition: sync.h:72
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void msgsCallback(const sensor_msgs::ImageConstPtr &l_img_msg, const sensor_msgs::ImageConstPtr &r_img_msg, const sensor_msgs::CameraInfoConstPtr &l_info_msg, const sensor_msgs::CameraInfoConstPtr &r_info_msg)
Definition: sync.cpp:70
#define ROS_WARN_STREAM(args)
message_filters::Synchronizer< SyncPolicy > SyncType
Definition: sync.h:81
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static Time now()
void syncCallback(const ros::TimerEvent &)
Definition: sync.cpp:82
string camera_
Timer to check the image sync
Definition: sync.h:74
ros::Publisher pub_info_
Definition: sync.h:86
ros::NodeHandle nh_
Definition: sync.h:66


avt_vimba_camera
Author(s): Miquel Massot , Allied Vision Technologies
autogenerated on Wed Jun 5 2019 22:22:40