pose_labeler_node.cpp

Go to the documentation of this file.

#include <string>

#include <boost/shared_ptr.hpp>

#include <ros/ros.h>
#include <nav_msgs/MapMetaData.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <topological_tools/pose_labeler.h>
#include <topological_tools/PoseLabel.h>

#include <stdlib.h>

#include <SDL/SDL_image.h>



namespace topological_tools
{
class PoseLabelerWrapper : public PoseLabeler
{
public:
    PoseLabelerWrapper ( const std::string& label_map_filename,
                         const std::string& pose_topic ) :
        PoseLabeler ( label_map_filename ),
        map_meta_sub_ ( nh_.subscribe ( "/map_metadata", 1, &PoseLabelerWrapper::mapMetadataCallback, this ) ),
        pose_sub_ ( nh_.subscribe ( pose_topic, 1, &PoseLabelerWrapper::poseCallback, this ) ),
        label_pub_ ( nh_.advertise<PoseLabel> ( "pose_label", 1, true ) ),
        last_label_ (),
        started_ ( false ) {}

    void mapMetadataCallback ( const nav_msgs::MapMetaDataConstPtr& msg )
    {
        setMapMetaData ( msg );
    }

    void poseCallback ( const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg )
    {
        PoseLabel currentLabel = getPoseLabel ( *msg );
        if ( !started_ || ( currentLabel.label != last_label_.label ) )
        {
            label_pub_.publish ( currentLabel );
            started_ = true;
            last_label_.label = currentLabel.label;
        }
    }

private:
    ros::NodeHandle nh_;

    ros::Subscriber map_meta_sub_;
    ros::Subscriber pose_sub_;

    ros::Publisher label_pub_;
    PoseLabel last_label_;
    bool started_;
};
}



int main ( int argc, char** argv )
{
    ros::init ( argc, argv, "pose_labeler_node" );

    if ( argc < 3 )
    {
        ROS_ERROR ( "\nUSAGE: pose_labeler_node <path to label map> <pose topic>\n" );
    }
    else
    {
        topological_tools::PoseLabelerWrapper plw ( argv[1], argv[2] );
        ros::spin();
    }

    return 0;
}