/// \brief Main callback which takes care of republishing.
///
/// This callback is called by the Synchronizer filter when
/// messages for both left/right images and cameras information
/// have been collected.
///
/// The callback the left image timestamps in all the other messages.
///
/// \param pub_l left image publisher (binded)
/// \param pub_cam_l left camera information publisher (binded)
/// \param pub_r right image publisher (binded)
/// \param pub_cam_r right camera information publisher (binded)
/// \param left left image
/// \param left_camera left camera information
/// \param right right image
/// \param right_camera right camera information
void imageCallback(image_transport::Publisher& pub_l,
                   ros::Publisher& pub_cam_l,
                   image_transport::Publisher& pub_r,
                   ros::Publisher& pub_cam_r,
                   const sensor_msgs::ImageConstPtr& left,
                   const sensor_msgs::CameraInfoConstPtr& left_camera,
                   const sensor_msgs::ImageConstPtr& right,
                   const sensor_msgs::CameraInfoConstPtr& right_camera
                  )
{
    sensor_msgs::Image left_(*left);
    sensor_msgs::Image right_(*right);
    sensor_msgs::CameraInfo left_camera_(*left_camera);
    sensor_msgs::CameraInfo right_camera_(*right_camera);

    // Fix timestamps.
    // FIXME: a warning should be issued if the differences between
    // timestamps are too important.
    left_camera_.header.stamp = left_.header.stamp;
    right_camera_.header.stamp = left_.header.stamp;
    right_.header.stamp = left_.header.stamp;

    pub_l.publish(left_);
    pub_cam_l.publish(left_camera_);
    pub_r.publish(right_);
    pub_cam_r.publish(right_camera_);
}
Exemple #2
0
	VertexType operator () (ScalarType u, ScalarType v) const {
		VertexType a = interpolate(u, left_(v), right_(v)),
			b = interpolate(v, bottom_(u), top_(u)),
			c = get_patch_value(u, v);

		return interpolate(.5,
			interpolate(-1, a, c),
			interpolate(-1, b, c)
		);
	}