FeatureTrackerNode.h
Go to the documentation of this file.
1 /*
2  * FeatureTrackerNode.h
3  *
4  * Author: roberto
5  *
6  * This is a modified implementation of the method for online estimation of kinematic structures described in our paper
7  * "Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors"
8  * (Martín-Martín and Brock, 2014).
9  * This implementation can be used to reproduce the results of the paper and to be applied to new research.
10  * The implementation allows also to be extended to perceive different information/models or to use additional sources of information.
11  * A detail explanation of the method and the system can be found in our paper.
12  *
13  * If you are using this implementation in your research, please consider citing our work:
14  *
15 @inproceedings{martinmartin_ip_iros_2014,
16 Title = {Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors},
17 Author = {Roberto {Mart\'in-Mart\'in} and Oliver Brock},
18 Booktitle = {Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems},
19 Pages = {2494-2501},
20 Year = {2014},
21 Location = {Chicago, Illinois, USA},
22 Note = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
23 Url = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
24 Projectname = {Interactive Perception}
25 }
26  * If you have questions or suggestions, contact us:
27  * roberto.martinmartin@tu-berlin.de
28  *
29  * Enjoy!
30  */
31 
32 #ifndef FEATURE_TRACKER_NODE_H_
33 #define FEATURE_TRACKER_NODE_H_
34 
35 #include <ros/ros.h>
36 #include <ros/callback_queue.h>
41 #include <sensor_msgs/PointCloud2.h>
42 #include <sensor_msgs/Image.h>
43 
44 #include <tf/tf.h>
45 
47 
48 #include <boost/thread.hpp>
49 
51 
52 // Dynamic reconfigure includes.
53 #include <dynamic_reconfigure/server.h>
54 #include <feature_tracker/FeatureTrackerDynReconfConfig.h>
55 
56 #include <std_msgs/Bool.h>
57 #include "omip_msgs/ShapeTrackerStates.h"
58 
59 namespace omip
60 {
67 template<class M>
69 {
70 public:
72  {
73  this->signalMessage(msg);
74  }
75 };
76 
85  public RecursiveEstimatorNodeInterface<ft_measurement_ros_t, ft_state_ros_t, FeatureTracker>
86 {
87  // Policies to synchorize point clouds, depth and RGB images from the sensor
90 
91 public:
92 
97 
98  virtual void AdvanceBagCallbackFromShapeReconstruction(const boost::shared_ptr<std_msgs::Bool const> &flag);
99 
100  virtual void AdvanceBagCallbackFromShapeTracker(const boost::shared_ptr<omip_msgs::ShapeTrackerStates const> &st_states);
101 
105  virtual ~FeatureTrackerNode();
106 
114  virtual void measurementCallback(const sensor_msgs::PointCloud2ConstPtr &pc_msg,
115  const sensor_msgs::ImageConstPtr &depth_image_msg,
116  const sensor_msgs::ImageConstPtr &rgb_image_msg);
117 
124  virtual void measurementCallback(const sensor_msgs::ImageConstPtr &depth_image_msg,
125  const sensor_msgs::ImageConstPtr &rgb_image_msg);
126 
132  virtual void measurementCallback(const boost::shared_ptr<ft_measurement_ros_t const> &ft_measurement_ros){}
133 
139  virtual void statePredictionCallback(const boost::shared_ptr<ft_state_ros_t const> &predicted_next_state);
140 
147  virtual void DynamicReconfigureCallback(feature_tracker::FeatureTrackerDynReconfConfig &config, uint32_t level);
148 
154  virtual void ReadRosBag();
155 
161  virtual void OcclusionMaskImgCallback(const sensor_msgs::ImageConstPtr &occlusion_mask_img);
162 
167  virtual void PublishTrackedFeaturesImg();
168 
173  virtual void PublishTrackedFeaturesWithPredictionMaskImg();
174 
179  virtual void PublishDepthEdgesImg();
180 
185  virtual void PublishPredictionMaskImg();
186 
191  virtual void PublishTrackingMaskImg();
192 
197  virtual void PublishDetectingMaskImg();
198 
203  virtual void PublishFullRGBDPC();
204 
205  virtual void PublishRGBImg();
206 
207  virtual void PublishDepthImg();
208 
213  virtual void PublishPredictedAndLastFeaturesImg();
214 
215  virtual void RepublishPredictedFeatLocation();
216 
217  virtual void run();
218 
219 protected:
220 
225  virtual void _publishState() const;
226 
231  virtual void _publishPredictedMeasurement() const;
232 
236  virtual void _ReadParameters();
237 
241  virtual void _InitializeVariables();
242 
246  virtual void _SubscribeAndAdvertiseTopics();
247 
249 
250  // Subscribers and synchronizer
257 
258  // Fake subscribers to capture images
263 
264  // Publishers
269 
271 
273 
274 
284 
285  sensor_msgs::CameraInfo _camera_info_msg;
286 
287  // PCL variables
288  PointCloudPCL::ConstPtr _full_rgbd_pc;
289  FeatureCloudPCLwc::Ptr _predicted_locations_pc;
290 
291  // OpenCV variables
295 
296  // Flags
311 
313 
315 
316  double _sensor_fps;
319 
320  std::string _ft_ns;
321 
322  std::string _tracker_type;
323  std::string _bag_file_name;
324  std::string _full_rgbd_pc_topic;
325  std::string _depth_img_topic;
326  std::string _rgb_img_topic;
328  std::string _ci_topic;
330 
332 
333  // Set up a dynamic reconfigure server.
334  // This should be done before reading parameter server values.
335  dynamic_reconfigure::Server<feature_tracker::FeatureTrackerDynReconfConfig> _dr_srv;
336  dynamic_reconfigure::Server<feature_tracker::FeatureTrackerDynReconfConfig>::CallbackType _dr_callback;
337 
340 
343 
345 };
346 }
347 
348 #endif /* FEATURE_TRACKER_NODE_H_ */
message_filters::Subscriber< sensor_msgs::PointCloud2 > _full_rgbd_pc_sub
dynamic_reconfigure::Server< feature_tracker::FeatureTrackerDynReconfConfig >::CallbackType _dr_callback
void newMessage(const boost::shared_ptr< M const > &msg)
void signalMessage(const MConstPtr &msg)
message_filters::Subscriber< sensor_msgs::Image > _rgb_img_sub
image_transport::Publisher _depth_img_pub
std::string _predicted_locations_pc_topic
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::Image, sensor_msgs::Image > FTrackerSyncPolicy
BagSubscriber< sensor_msgs::Image > _bag_depth_img_sub
ros::Publisher _shutdown_publisher
dynamic_reconfigure::Server< feature_tracker::FeatureTrackerDynReconfConfig > _dr_srv
FeatureCloudPCLwc::Ptr _predicted_locations_pc
cv_bridge::CvImagePtr _cv_ptr_depth
BagSubscriber< sensor_msgs::Image > _bag_rgb_img_sub
image_transport::Publisher _detecting_mask_img_pub
image_transport::Publisher _prediction_mask_img_pub
sensor_msgs::CameraInfo _camera_info_msg
ros::Publisher _full_rgbd_pc_repub
image_transport::Publisher _tracked_feats_with_pm_img_pub
image_transport::Publisher _depth_edges_img_pub
message_filters::Synchronizer< FTrackerLightSyncPolicy > * _light_synchronizer
message_filters::Synchronizer< FTrackerSyncPolicy > * _synchronizer
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image > FTrackerLightSyncPolicy
ros::Subscriber _occlusion_mask_img_sub
FeatureTrackerNode class Connects to the ROS communication system to get messages from the sensors an...
image_transport::Publisher _predicted_and_past_feats_img_pub
virtual void measurementCallback(const boost::shared_ptr< ft_measurement_ros_t const > &ft_measurement_ros)
Empty measurement callback, just to implement correctly the RENode interface.
image_transport::Publisher _rgb_img_pub
ros::Publisher _pred_feat_locs_repub
PointCloudPCL::ConstPtr _full_rgbd_pc
image_transport::Publisher _tracked_feats_img_pub
image_transport::ImageTransport _image_transport
image_transport::Publisher _tracking_mask_img_pub
BagSubscriber< sensor_msgs::PointCloud2 > _bag_full_rgbd_pc_sub
void run(ClassLoader *loader)
cv_bridge::CvImagePtr _cv_ptr_occlusion_msk
message_filters::Subscriber< sensor_msgs::Image > _depth_img_sub
cv_bridge::CvImagePtr _cv_ptr_rgb


feature_tracker
Author(s): Roberto Martín-Martín
autogenerated on Mon Jun 10 2019 14:06:08