RosLoService.h

Go to the documentation of this file.

/**********************************************************************************************/
#include <ros/node_handle.h>
#include <ros/time.h>
#include <sstream>

#include <vision_msgs/partial_lo.h>
#include <vision_srvs/srvjlo.h>
#include <vision_srvs/register_jlo_callback.h>
#include <lo/ServiceInterface.h>

#include "tf/tfMessage.h"
#include "tf/tf.h"

#include <set>
#include <queue>
#include <string>

class RosLoService : public jlo::ServiceInterface
{
public:
  RosLoService (const char* nodename, ros::NodeHandle &n, std::string configFile);
  ~RosLoService ();


  class PubParentFilterStruct
  {
  public:
    PubParentFilterStruct()
      : parent_id(0), last_value_set(false) {}
    PubParentFilterStruct( std::vector<ros::Publisher*> pub, unsigned long parent, std::vector<double> filt)
       : publisher(pub), parent_id(parent), filter(filt), last_value_set(false) {}
    std::vector<ros::Publisher*> publisher;
    unsigned long                parent_id;
    std::vector<double>           filter;
    std::vector<double> last_val;
    bool last_value_set;

  };

  bool ServiceCallback(vision_srvs::srvjlo::Request& request, vision_srvs::srvjlo::Response&  answer);

  bool CallbackRegisterService(vision_srvs::register_jlo_callback::Request& request, vision_srvs::register_jlo_callback::Response&  answer);
  void UpdateEventHandler();

  bool PublishUpdate(unsigned long id, PubParentFilterStruct& obj_to_pub);
  static void UpdateEventNotifier(unsigned long object_id);
private:
  ros::Subscriber tf_subscription;
  ros::ServiceServer located_object_service;
  std::set<std::string> tf_blacklist;

/*  ros::ServiceServer located_object_callback_reagister_service;
*/
  std::map<unsigned long, PubParentFilterStruct > m_jlotopicMap;
  std::map<unsigned long, unsigned long> m_parentToRegisteredJlo;
  std::map<std::string, ros::Publisher> m_topicPublisherMap;
  static std::queue<unsigned long> m_queueOfLosToPublish;

  void tf_subscription_callback(const tf::tfMessage::ConstPtr &msg_in_);
};