ObjectTFBroadcaster.h

Go to the documentation of this file.

#ifndef OBJECT_MSGS_TOOLS_OBJECTTFBROADCASTER_H
#define OBJECT_MSGS_TOOLS_OBJECTTFBROADCASTER_H

#include <ros/ros.h>
#include <object_msgs/Object.h>
#include <object_msgs/RegisterObject.h>
#include <tf/transform_broadcaster.h>
#include <boost/thread.hpp>

namespace object_msgs_tools {

class ObjectTFBroadcaster
{
public:

    ObjectTFBroadcaster(
        ros::NodeHandle& n,
        const std::string& register_object_service,
        unsigned int publish_tf_rate,
        unsigned int query_object_info_rate,
        const std::string& object_topic,
        const std::string& object_service);

    bool isRegistered(const std::string& name) const; 

    int registerObject(const std::string& name, bool printMsgs = true); 

private:

    bool registerObjectService(object_msgs::RegisterObject::Request &req,
                               object_msgs::RegisterObject::Response &res);

    bool queryObjectPose(const std::string& name, object_msgs::Object& obj, bool printErrors = true);

    void sendTF(const geometry_msgs::PoseStamped& pose, const std::string& frame_name);

    bool updateObject(const object_msgs::Object& obj);
   
    void objectCallback(const object_msgs::Object& obj);

    void publishTFEvent(const ros::TimerEvent& e);

    void queryObjectPoseEvent(const ros::TimerEvent& e);

    // all object poses which have been registered
    // and will be continuously re-published on /tf. 
    std::map<std::string, geometry_msgs::PoseStamped> obj_poses;
    mutable boost::recursive_mutex obj_poses_mutex;

    ros::ServiceServer register_object_srv;
    ros::ServiceClient object_info_srv;
    ros::Subscriber object_info_sub;

    ros::NodeHandle node;
            
    tf::TransformBroadcaster tf_broadcaster;
    ros::Timer publish_tf_timer;
    ros::Timer query_object_info_timer;
};

}

#endif  // OBJECT_MSGS_TOOLS_OBJECTTFBROADCASTER_H