subscriber/publisher new API

include/factory_publisher.h

@@ -156,15 +156,19 @@ namespace wolf
  *
  * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
  */
-    class Publisher;
-    typedef Factory<Publisher> FactoryPublisher;
+class Publisher;
+typedef Factory<Publisher,
+                const std::string&,
+                const ParamsServer&,
+                const ProblemPtr,
+                ros::NodeHandle&> FactoryPublisher;
+
 template<>
 inline std::string FactoryPublisher::getClass() const
 {
   return "FactoryPublisher";
 }
 
-
 #define WOLF_REGISTER_PUBLISHER(PublisherType)                        \
     namespace{ const bool WOLF_UNUSED PublisherType##Registered =      \
             wolf::FactoryPublisher::registerCreator(#PublisherType, PublisherType::create); } \

include/factory_subscriber.h

@@ -160,7 +160,8 @@ namespace wolf
     typedef Factory<Subscriber,
                     const std::string&,
                     const ParamsServer&,
-                    const SensorBasePtr> FactorySubscriber;
+                    const SensorBasePtr,
+                    ros::NodeHandle&> FactorySubscriber;
 template<>
 inline std::string FactorySubscriber::getClass() const
 {

include/publisher.h

@@ -10,53 +10,88 @@
  **************************/
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
+#include "factory_publisher.h"
 
 namespace wolf
 {
 WOLF_PTR_TYPEDEFS(Publisher);
 
 /*
- * Macro for defining Autoconf subscriber creator for WOLF's high level API.
+ * Macro for defining Autoconf publisher creator for WOLF's high level API.
  *
- * Place a call to this macro inside your class declaration (in the subscriber_class.h file),
+ * Place a call to this macro inside your class declaration (in the publisher_class.h file),
  * preferably just after the constructors.
  *
- * In order to use this macro, the derived subscriber class, PublisherClass,
+ * In order to use this macro, the derived publisher class, PublisherClass,
  * must have a constructor available with the API:
  *
  *   PublisherClass(const std::string& _unique_name,
- *                   const ParamsServer& _server,
- *                   const SensorBasePtr _sensor_ptr);
+ *                  const ParamsServer& _server,
+ *                  const ProblemPtr _problem);
  */
-#define WOLF_PUBLISHER_CREATE(PublisherClass)                                    \
-static PublisherPtr create(const std::string& _unique_name,                      \
-                            const ParamsServer& _server)                         \
-{                                                                                \
-    return std::make_shared<PublisherClass>(_unique_name, _server); \
-}                                                                                \
+#define WOLF_PUBLISHER_CREATE(PublisherClass)                                               \
+        static PublisherPtr create(const std::string& _unique_name,                         \
+                                   const ParamsServer& _server,                             \
+                                   const ProblemPtr _problem,                               \
+                                   ros::NodeHandle& _nh)                                    \
+                                   {                                                        \
+    PublisherPtr pub = std::make_shared<PublisherClass>(_unique_name, _server, _problem);   \
+    pub->initialize(_nh, pub->getTopic());                                                  \
+    return pub;                                                                             \
+}                                                                                           \
 
 class Publisher
 {
-  public:
+    public:
 
-    Publisher(const std::string& _unique_name,
-              const ParamsServer& _server)
-      : period_(1)
-      , last_publish_time_(ros::Time(0))
-    {};
+        Publisher(const std::string& _unique_name,
+                  const ParamsServer& _server,
+                  const ProblemPtr _problem) :
+                      problem_(_problem),
+                      last_publish_time_(ros::Time(0)),
+                      prefix_("ROS publisher/" + _unique_name)
+        {
+            period_ = _server.getParam<double>(prefix_ + "/period");
+            topic_  = _server.getParam<std::string>(prefix_ + "/topic");
+        };
 
-    virtual ~Publisher(){};
+        virtual ~Publisher(){};
 
-    virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0;
+        virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0;
 
-    virtual void publish(const ProblemPtr problem) = 0;
+        virtual void publish() final;
 
-    double period_;
-    ros::Time last_publish_time_;
+        virtual void publishDerived() = 0;
 
-  protected:
+        virtual bool ready();
 
-    ros::Publisher publisher_;
+        std::string getTopic() const;
+
+    protected:
+
+        ProblemPtr problem_;
+        ros::Publisher publisher_;
+        double period_;
+        ros::Time last_publish_time_;
+        std::string prefix_;
+        std::string topic_;
 };
+
+inline void Publisher::publish()
+{
+    last_publish_time_ = ros::Time::now();
+    publishDerived();
+}
+
+inline bool Publisher::ready()
+{
+    return (ros::Time::now() - last_publish_time_).toSec() > period_;
+}
+
+inline std::string Publisher::getTopic() const
+{
+    return topic_;
+}
+
 }  // namespace wolf
 #endif

include/publisher_pose.h

@@ -6,7 +6,6 @@
  **************************/
 #include "core/problem/problem.h"
 
-#include "factory_publisher.h"
 #include "publisher.h"
 
 namespace wolf
@@ -18,12 +17,15 @@ class PublisherPose: public Publisher
 
     public:
         PublisherPose(const std::string& _unique_name,
-                      const ParamsServer& _server);
+                      const ParamsServer& _server,
+                      const ProblemPtr _problem);
         WOLF_PUBLISHER_CREATE(PublisherPose);
+
         virtual ~PublisherPose(){};
+
         void initialize(ros::NodeHandle &nh, const std::string& topic) override;
-        void publish(const ProblemPtr problem) override;
-        static std::shared_ptr<Publisher> create();
+
+        void publishDerived() override;
 };
 
 WOLF_REGISTER_PUBLISHER(PublisherPose)

include/subscriber.h

@@ -11,7 +11,6 @@
  *      ROS includes      *
  **************************/
 #include <ros/ros.h>
-#include <nav_msgs/Odometry.h>
 
 namespace wolf {
 WOLF_PTR_TYPEDEFS(Subscriber);
@@ -29,13 +28,16 @@ WOLF_PTR_TYPEDEFS(Subscriber);
  *                   const ParamsServer& _server,
  *                   const SensorBasePtr _sensor_ptr);
  */
-#define WOLF_SUBSCRIBER_CREATE(SubscriberClass)                                     \
-static SubscriberPtr create(const std::string& _unique_name,                        \
-                            const ParamsServer& _server,                            \
-                            const SensorBasePtr _sensor_ptr)                        \
-{                                                                                   \
-    return std::make_shared<SubscriberClass>(_unique_name, _server, _sensor_ptr);   \
-}                                                                                   \
+#define WOLF_SUBSCRIBER_CREATE(SubscriberClass)                                                 \
+static SubscriberPtr create(const std::string& _unique_name,                                    \
+                            const ParamsServer& _server,                                        \
+                            const SensorBasePtr _sensor_ptr,                                    \
+                            ros::NodeHandle& _nh)                                               \
+{                                                                                               \
+    SubscriberPtr sub = std::make_shared<SubscriberClass>(_unique_name, _server, _sensor_ptr);  \
+    sub->initialize(_nh, sub->getTopic());                                                      \
+    return sub;                                                                                 \
+}                                                                                               \
 
 class Subscriber
 {
@@ -43,6 +45,7 @@ class Subscriber
         //wolf
         SensorBasePtr sensor_ptr_;
         std::string prefix_;
+        std::string topic_;
 
         // ros
         ros::Subscriber sub_;
@@ -54,10 +57,20 @@ class Subscriber
             sensor_ptr_(_sensor_ptr),
             prefix_("ROS subscriber/" + _unique_name)
         {
+            topic_  = _server.getParam<std::string>(prefix_ + "/topic");
         }
+
         virtual ~Subscriber(){};
 
-        virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic) = 0;
+        virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0;
+
+        std::string getTopic() const;
 };
+
+inline std::string Subscriber::getTopic() const
+{
+    return topic_;
+}
+
 }
 #endif

include/subscriber_diffdrive.h

@@ -2,7 +2,7 @@
  *      WOLF includes     *
  **************************/
 #include <core/common/wolf.h>
-#include <core/utils/params_server.h>
+#include "subscriber.h"
 
 /**************************
  *      ROS includes      *
@@ -11,8 +11,6 @@
 #include <nav_msgs/Odometry.h>
 #include "sensor_msgs/JointState.h"
 
-#include "subscriber.h"
-
 namespace wolf
 {
 class SubscriberDiffdrive : public Subscriber
@@ -31,7 +29,7 @@ class SubscriberDiffdrive : public Subscriber
                         const SensorBasePtr _sensor_ptr);
     WOLF_SUBSCRIBER_CREATE(SubscriberDiffdrive);
 
-    virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic);
+    virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
 
     void callback(const sensor_msgs::JointState::ConstPtr& msg);
 };

include/subscriber_odom2d.h

@@ -27,7 +27,7 @@ class SubscriberOdom2d : public Subscriber
                      const SensorBasePtr _sensor_ptr);
     WOLF_SUBSCRIBER_CREATE(SubscriberOdom2d);
 
-    virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic);
+    virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
 
     void callback(const nav_msgs::Odometry::ConstPtr& msg);
 };

src/node.cpp

@@ -52,9 +52,7 @@ WolfRosNode::WolfRosNode()
         std::string topic      = it["topic"];
         std::string sensor     = it["sensor_name"];
         WOLF_TRACE("From sensor {" + sensor + "} subscribing {" + subscriber + "} to {" + topic + "} topic");
-        auto subscriber_wrapper = FactorySubscriber::create(subscriber, topic, server, problem_ptr_->getSensor(sensor));
-        subscribers_.push_back(subscriber_wrapper);
-        subscribers_.back()->initSubscriber(nh_, topic);
+        subscribers_.push_back(FactorySubscriber::create(subscriber, topic, server, problem_ptr_->getSensor(sensor), nh_));
     }
 
     // // ROS VISUALIZER
@@ -72,12 +70,8 @@ WolfRosNode::WolfRosNode()
     {
         for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS publisher"))
         {
-            std::string pub = it["type"];
-            WOLF_INFO("Pub: ", pub);
-            auto publisher = FactoryPublisher::create(pub);
-            publisher->period_ = converter<double>::convert(it["period"]);
-            publishers_.push_back(publisher);
-            publishers_.back()->initialize(nh_,it["topic"]);
+            WOLF_INFO("Pub: ", it["type"]);
+            publishers_.push_back(FactoryPublisher::create(it["type"], it["topic"], server, problem_ptr_, nh_));
         }
     }
     catch (MissingValueException& e)
@@ -240,11 +234,8 @@ int main(int argc, char **argv)
 
         // publish periodically
         for(auto pub : wolf_node.publishers_)
-            if ((ros::Time::now() - pub->last_publish_time_).toSec() >= pub->period_)
-            {
-                pub->publish(wolf_node.problem_ptr_);
-                pub->last_publish_time_ = ros::Time::now();
-            }
+            if (pub->ready())
+                pub->publish();
 
         // execute pending callbacks
         ros::spinOnce();

src/publisher_pose.cpp

@@ -10,9 +10,10 @@
 namespace wolf
 {
 
-    PublisherPose::PublisherPose(const std::string& _unique_name,
-                                 const ParamsServer& _server) :
-    Publisher(_unique_name, _server)
+PublisherPose::PublisherPose(const std::string& _unique_name,
+                             const ParamsServer& _server,
+                             const ProblemPtr _problem) :
+        Publisher(_unique_name, _server, _problem)
 {
 }
 
@@ -22,10 +23,10 @@ void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
     nh.param<std::string>("map_frame_id", map_frame_id_, "map");
 }
 
-void PublisherPose::publish(const ProblemPtr _problem)
+void PublisherPose::publishDerived()
 {
-    VectorComposite current_state = _problem->getState("PO");
-    TimeStamp loc_ts = _problem->getTimeStamp();
+    VectorComposite current_state = problem_->getState("PO");
+    TimeStamp loc_ts = problem_->getTimeStamp();
 
     // state not ready
     if (current_state.count("P") == 0 or
@@ -41,7 +42,7 @@ void PublisherPose::publish(const ProblemPtr _problem)
     msg.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
 
     // 2D
-    if (_problem->getDim() == 2)
+    if (problem_->getDim() == 2)
     {
         msg.pose.pose.position.x = current_state["P"](0);
         msg.pose.pose.position.y = current_state["P"](1);

src/subscriber_diffdrive.cpp

@@ -2,11 +2,8 @@
  *      WOLF includes     *
  **************************/
 #include <core/capture/capture_diff_drive.h>
-//#include <core/processor/processor_diff_drive.h>
-//#include <core/processor/processor_motion.h>
 #include <core/sensor/sensor_diff_drive.h>
 
-// #include "Eigen/src/Core/Matrix.h"
 #include "core/math/rotations.h"
 #include "subscriber_diffdrive.h"
 
@@ -23,7 +20,7 @@ SubscriberDiffdrive::SubscriberDiffdrive(const std::string& _unique_name,
   ticks_cov_factor_ = std::static_pointer_cast<SensorDiffDrive>(_sensor_ptr)->getParams()->ticks_cov_factor;
 }
 
-void SubscriberDiffdrive::initSubscriber(ros::NodeHandle& nh, const std::string& topic)
+void SubscriberDiffdrive::initialize(ros::NodeHandle& nh, const std::string& topic)
 {
     sub_ = nh.subscribe(topic, 100, &SubscriberDiffdrive::callback, this);
 }

src/subscriber_odom2d.cpp

@@ -33,7 +33,7 @@ SubscriberOdom2d::SubscriberOdom2d(const std::string& _unique_name,
 {
 }
 
-void SubscriberOdom2d::initSubscriber(ros::NodeHandle& nh, const std::string& topic)
+void SubscriberOdom2d::initialize(ros::NodeHandle& nh, const std::string& topic)
 {
     sub_ = nh.subscribe(topic, 100, &SubscriberOdom2d::callback, this);
 }