topic_with_transform.h
Go to the documentation of this file.
00001 
00008 #ifndef __TOPIC_WITH_TRANSFORM_H
00009 #define __TOPIC_WITH_TRANSFORM_H
00010 
00011 #include <string>
00012 #include <vector>
00013 #include <memory>
00014 #include <message_filters/subscriber.h>
00015 #include <tf/message_filter.h>
00016 #include <tf/transform_listener.h>
00017 #include <boost/shared_ptr.hpp>
00018 
00023 // TODO: make this class inner
00024 template <typename MType>
00025 class TopicObserver { // iface
00026 public: // methods
00027   virtual void handle_transformed_msg(const boost::shared_ptr<MType>,
00028                                       const tf::StampedTransform&) = 0;
00029 };
00030 
00034 // TODO: add scan drop
00035 template <typename MsgType>
00036 class TopicWithTransform {
00037   /* NB: wasn't able to implement with TF2 (ROS jade),
00038          probably because of deadlock
00039            (https://github.com/ros/geometry2/pull/144)
00040          Try TF2 with proposed patch.
00041    */
00042 public: // methods
00043   // TODO: copy, move ctrs, dtor
00044   TopicWithTransform(ros::NodeHandle nh,
00045                      const std::string& topic_name,
00046                      const std::string& target_frame,
00047                      const double buffer_duration,
00048                      const double tf_filter_queue_size,
00049                      const double subscribers_queue_size):
00050      FILTER_QUEUE_SZ(tf_filter_queue_size),
00051      SUBSCR_QUEUE_SZ(subscribers_queue_size),
00052     _target_frame{target_frame},
00053     _subscr{nh, topic_name, SUBSCR_QUEUE_SZ},
00054     _tf_lsnr{ros::Duration(buffer_duration)},
00055     _msg_flt{new tf::MessageFilter<MsgType>{
00056       _subscr, _tf_lsnr, _target_frame, FILTER_QUEUE_SZ}} {
00057       _msg_flt->registerCallback(&TopicWithTransform::transformed_msg_cb,
00058                                   this);
00059   }
00060 
00065   void subscribe(std::shared_ptr<TopicObserver<MsgType>> obs) {
00066     _observers.push_back(obs);
00067   }
00068 private: // consts
00069 
00070   const uint32_t FILTER_QUEUE_SZ = 10000; // elems
00071   const uint32_t SUBSCR_QUEUE_SZ = 10000; // elems
00072 private: // methods
00073   void transformed_msg_cb(const boost::shared_ptr<MsgType> msg) {
00074     tf::StampedTransform transform;
00075     std::string msg_frame_id =
00076       ros::message_traits::FrameId<MsgType>::value(*msg);
00077     ros::Time msg_time =
00078       ros::message_traits::TimeStamp<MsgType>::value(*msg);
00079 
00080     try{
00081       // NB: using last available msg (time = 0) causes issues
00082       //     on running with higher rate.
00083       _tf_lsnr.lookupTransform(_target_frame, msg_frame_id,
00084                                msg_time, transform);
00085     }
00086     catch (tf::TransformException ex){
00087       ROS_ERROR("Transform retrieval failed. %s",ex.what());
00088       return;
00089     }
00090 
00091     for (auto obs : _observers) {
00092       if (auto obs_ptr = obs.lock()) {
00093         obs_ptr->handle_transformed_msg(msg, transform);
00094       }
00095     }
00096   }
00097 private: // fields
00098   std::string _target_frame;
00099   message_filters::Subscriber<MsgType> _subscr;
00100   tf::TransformListener _tf_lsnr;
00101   std::unique_ptr<tf::MessageFilter<MsgType>> _msg_flt;
00102   std::vector<std::weak_ptr<TopicObserver<MsgType>>> _observers;
00103 };
00104 
00105 #endif // macro guard


tiny_slam
Author(s):
autogenerated on Thu Jun 6 2019 17:44:57