ShapeTrackerNode.h

Go to the documentation of this file.

/*
 * ShapeTrackerNode.h
 *
 *      Author: roberto
 *
 * This is a modified implementation of the method for online estimation of kinematic structures described in our paper
 * "Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors"
 * (Martín-Martín and Brock, 2014) and the extension to segment, reconstruct and track shapes introduced in our paper
 * "An Integrated Approach to Visual Perception of Articulated Objects" (Martín-Martín, Höfer and Brock, 2016)
 * This implementation can be used to reproduce the results of the paper and to be applied to new research.
 * The implementation allows also to be extended to perceive different information/models or to use additional sources of information.
 * A detail explanation of the method and the system can be found in our paper.
 *
 * If you are using this implementation in your research, please consider citing our work:
 *
@inproceedings{martinmartin_ip_iros_2014,
Title = {Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors},
Author = {Roberto {Mart\'in-Mart\'in} and Oliver Brock},
Booktitle = {Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems},
Pages = {2494-2501},
Year = {2014},
Location = {Chicago, Illinois, USA},
Note = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
Url = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
Projectname = {Interactive Perception}
}

@inproceedings{martinmartin_hoefer_iros_2016,
Title = {An Integrated Approach to Visual Perception of Articulated Objects},
Author = {Roberto {Mart\'{i}n-Mart\'{i}n} and Sebastian H\"ofer and Oliver Brock},
Booktitle = {Proceedings of the IEEE International Conference on Robotics and Automation},
Pages = {5091 - 5097},
Year = {2016},
Doi = {10.1109/ICRA.2016.7487714},
Location = {Stockholm, Sweden},
Month = {05},
Note = {http://www.redaktion.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martin_hoefer_15_iros_sr_opt.pdf},
Url = {http://www.redaktion.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martin_hoefer_15_iros_sr_opt.pdf},
Url2 = {http://ieeexplore.ieee.org/xpl/articleDetails.jsp?tp=&arnumber=7487714},
Projectname = {Interactive Perception}
}
 * If you have questions or suggestions, contact us:
 * roberto.martinmartin@tu-berlin.de
 *
 * Enjoy!
 */

#ifndef SHAPE_TRACKER_NODE_H_
#define SHAPE_TRACKER_NODE_H_

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/subscriber.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>

#include <rosbag/bag.h>
#include <tf/tf.h>

#include <boost/thread.hpp>

#include <omip_common/OMIPTypeDefs.h>
#include <omip_msgs/ShapeModels.h>

// Dynamic reconfigure includes.
#include <dynamic_reconfigure/server.h>

#include <omip_common/OMIPTypeDefs.h>

//ROS and OpenCV
#include <opencv2/core/core.hpp>
#include <camera_info_manager/camera_info_manager.h>

#include <omip_common/OMIPUtils.h>

#include <shape_tracker/ShapeTracker.h>

#include <std_msgs/Empty.h>


namespace omip
{

class ShapeTrackerNode
{
    // Policies to synchorize point clouds, and the RBP from the RBT
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, omip::rbt_state_t> STSyncPolicy;

    typedef std::map<omip::RB_id_t, omip::ShapeTrackerPtr > shape_trackers_map_t;

public:

    ShapeTrackerNode();

    virtual ~ShapeTrackerNode();

    virtual void CameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &ci_msg);

    virtual void ShapeModelsCallback(const omip_msgs::ShapeModelsConstPtr &models_msg);

    virtual void RigibBodyMotionsAndPCCallback(const sensor_msgs::PointCloud2ConstPtr &pc_msg, const boost::shared_ptr<omip::rbt_state_t const> &poses_and_vels);

    virtual void TrackerNodeQuitCallback(const std_msgs::EmptyConstPtr &msg);

    virtual bool getActive() const
    {
        return this->_active;
    }

protected:

    shape_trackers_map_t                                                        _rb_trackers;
    ros::NodeHandle                                                             _node_handle;
    ros::Publisher                                                              _st_state_pub;

    message_filters::Subscriber<sensor_msgs::PointCloud2>                       _rgbd_pc_subscriber;
    message_filters::Subscriber<omip::rbt_state_t>                              _poses_and_vels_subscriber;
    message_filters::Synchronizer<STSyncPolicy>*                                _synchronizer;

    ros::Subscriber                      _shape_models_subscriber;

    ros::Subscriber                                                             _ci_sub;
    sensor_msgs::CameraInfo _ci;

    shape_model_selector_t _model_type_to_listen;

    ros::Subscriber _node_quit_subscriber;

    bool _active;

    template<class T>
    bool getROSParameter(std::string param_name, T & param_container)
    {
        if (!(this->_node_handle.getParam(param_name, param_container)))
        {
            ROS_ERROR_NAMED("ShapeTrackerNode.getROSParameter", "The parameter %s can not be found.", param_name.c_str());
            throw(std::string("[ShapeTrackerNode.getROSParameter] The parameter can not be found. Parameter name: ") + param_name);
            return false;
        }
        else
            return true;
    }
};
}

#endif /* FEATURE_TRACKER_NODE_H_ */