From a63b561906c04772c3ae46489dc1c49d022d8ce4 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 12 Jun 2020 16:22:44 +0200
Subject: [PATCH] subscriber/publisher new API
---
include/factory_publisher.h | 10 ++--
include/factory_subscriber.h | 3 +-
include/publisher.h | 83 ++++++++++++++++++++++++----------
include/publisher_pose.h | 10 ++--
include/subscriber.h | 31 +++++++++----
include/subscriber_diffdrive.h | 6 +--
include/subscriber_odom2d.h | 2 +-
src/node.cpp | 19 ++------
src/publisher_pose.cpp | 15 +++---
src/subscriber_diffdrive.cpp | 5 +-
src/subscriber_odom2d.cpp | 2 +-
11 files changed, 114 insertions(+), 72 deletions(-)
diff --git a/include/factory_publisher.h b/include/factory_publisher.h
index 246d660..a10490b 100644
--- a/include/factory_publisher.h
+++ b/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); } \
diff --git a/include/factory_subscriber.h b/include/factory_subscriber.h
index 0854150..c45b96e 100644
--- a/include/factory_subscriber.h
+++ b/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
{
diff --git a/include/publisher.h b/include/publisher.h
index fb42ae5..d962d61 100644
--- a/include/publisher.h
+++ b/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
diff --git a/include/publisher_pose.h b/include/publisher_pose.h
index f13eae5..f5a0442 100644
--- a/include/publisher_pose.h
+++ b/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)
diff --git a/include/subscriber.h b/include/subscriber.h
index eddd420..cab5ad7 100644
--- a/include/subscriber.h
+++ b/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
diff --git a/include/subscriber_diffdrive.h b/include/subscriber_diffdrive.h
index 5783bd0..2a4ce90 100644
--- a/include/subscriber_diffdrive.h
+++ b/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);
};
diff --git a/include/subscriber_odom2d.h b/include/subscriber_odom2d.h
index 849bbd1..50a3a7d 100644
--- a/include/subscriber_odom2d.h
+++ b/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);
};
diff --git a/src/node.cpp b/src/node.cpp
index dc953fa..63a4835 100644
--- a/src/node.cpp
+++ b/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();
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index 046dd97..ee95b54 100644
--- a/src/publisher_pose.cpp
+++ b/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);
diff --git a/src/subscriber_diffdrive.cpp b/src/subscriber_diffdrive.cpp
index 9463c06..7c30b79 100644
--- a/src/subscriber_diffdrive.cpp
+++ b/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);
}
diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp
index 90a02c6..b7c3df6 100644
--- a/src/subscriber_odom2d.cpp
+++ b/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);
}
--
GitLab