00001 #ifndef POLLED_CAMERA_PUBLICATION_SERVER_H
00002 #define POLLED_CAMERA_PUBLICATION_SERVER_H
00003
00004 #include <ros/ros.h>
00005 #include <sensor_msgs/Image.h>
00006 #include <sensor_msgs/CameraInfo.h>
00007 #include "polled_camera/GetPolledImage.h"
00008
00009 namespace polled_camera {
00010
00032 class PublicationServer
00033 {
00034 public:
00035 typedef boost::function<void (polled_camera::GetPolledImage::Request&,
00036 polled_camera::GetPolledImage::Response&,
00037 sensor_msgs::Image&,
00038 sensor_msgs::CameraInfo&)> DriverCallback;
00039
00040 PublicationServer() {}
00041
00045 void shutdown();
00046
00047 std::string getService() const;
00048
00049 operator void*() const;
00050 bool operator< (const PublicationServer& rhs) const { return impl_ < rhs.impl_; }
00051 bool operator==(const PublicationServer& rhs) const { return impl_ == rhs.impl_; }
00052 bool operator!=(const PublicationServer& rhs) const { return impl_ != rhs.impl_; }
00053
00054 private:
00055 PublicationServer(const std::string& service, ros::NodeHandle& nh,
00056 const DriverCallback& cb, const ros::VoidPtr& tracked_object);
00057
00058 class Impl;
00059
00060 boost::shared_ptr<Impl> impl_;
00061
00062 friend
00063 PublicationServer advertise(ros::NodeHandle&, const std::string&, const DriverCallback&,
00064 const ros::VoidPtr&);
00065 };
00066
00070 PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
00071 const PublicationServer::DriverCallback& cb,
00072 const ros::VoidPtr& tracked_object = ros::VoidPtr());
00073
00077 template<class T>
00078 PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
00079 void(T::*fp)(polled_camera::GetPolledImage::Request&,
00080 polled_camera::GetPolledImage::Response&,
00081 sensor_msgs::Image&, sensor_msgs::CameraInfo&),
00082 T* obj)
00083 {
00084 return advertise(nh, service, boost::bind(fp, obj, _1, _2, _3, _4));
00085 }
00086
00090 template<class T>
00091 PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
00092 void(T::*fp)(polled_camera::GetPolledImage::Request&,
00093 polled_camera::GetPolledImage::Response&,
00094 sensor_msgs::Image&, sensor_msgs::CameraInfo&),
00095 const boost::shared_ptr<T>& obj)
00096 {
00097 return advertise(nh, service, boost::bind(fp, obj.get(), _1, _2, _3, _4), obj);
00098 }
00099
00100 }
00101
00102 #endif