diff --git a/include/subscriber.h b/include/subscriber.h index 30f9089840c16c4efd51f21206e1b17e06aa8e0c..eddd420be39186d4baba2c02eaac4a4faae9c0f9 100644 --- a/include/subscriber.h +++ b/include/subscriber.h @@ -16,18 +16,43 @@ namespace wolf { WOLF_PTR_TYPEDEFS(Subscriber); +/* + * Macro for defining Autoconf subscriber creator for WOLF's high level API. + * + * Place a call to this macro inside your class declaration (in the subscriber_class.h file), + * preferably just after the constructors. + * + * In order to use this macro, the derived subscriber class, SubscriberClass, + * must have a constructor available with the API: + * + * SubscriberClass(const std::string& _unique_name, + * 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); \ +} \ + class Subscriber { protected: //wolf SensorBasePtr sensor_ptr_; + std::string prefix_; // ros ros::Subscriber sub_; public: - Subscriber(const SensorBasePtr& sensor_ptr) : - sensor_ptr_(sensor_ptr) + Subscriber(const std::string& _unique_name, + const ParamsServer& _server, + const SensorBasePtr _sensor_ptr) : + sensor_ptr_(_sensor_ptr), + prefix_("ROS subscriber/" + _unique_name) { } virtual ~Subscriber(){}; diff --git a/include/subscriber_diffdrive.h b/include/subscriber_diffdrive.h index f2c0fd2c975af7450a096d5a1478459d4ce022ea..5783bd05cee325ff8bc709609b10519dfac9472e 100644 --- a/include/subscriber_diffdrive.h +++ b/include/subscriber_diffdrive.h @@ -26,13 +26,14 @@ class SubscriberDiffdrive : public Subscriber public: - SubscriberDiffdrive(const SensorBasePtr& sensor_ptr); + SubscriberDiffdrive(const std::string& _unique_name, + const ParamsServer& _server, + const SensorBasePtr _sensor_ptr); + WOLF_SUBSCRIBER_CREATE(SubscriberDiffdrive); virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic); void callback(const sensor_msgs::JointState::ConstPtr& msg); - - static std::shared_ptr<Subscriber> create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr); }; WOLF_REGISTER_SUBSCRIBER(SubscriberDiffdrive) diff --git a/include/subscriber_odom2d.h b/include/subscriber_odom2d.h index 72aa775dfeef98f3105ef097601bdcb7ac89ac40..849bbd17b480d3e74c44808e43e577033afdd7ce 100644 --- a/include/subscriber_odom2d.h +++ b/include/subscriber_odom2d.h @@ -22,13 +22,14 @@ class SubscriberOdom2d : public Subscriber public: - SubscriberOdom2d(const SensorBasePtr& sensor_ptr); + SubscriberOdom2d(const std::string& _unique_name, + const ParamsServer& _server, + const SensorBasePtr _sensor_ptr); + WOLF_SUBSCRIBER_CREATE(SubscriberOdom2d); virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic); void callback(const nav_msgs::Odometry::ConstPtr& msg); - - static std::shared_ptr<Subscriber> create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr); }; WOLF_REGISTER_SUBSCRIBER(SubscriberOdom2d) diff --git a/src/subscriber_diffdrive.cpp b/src/subscriber_diffdrive.cpp index 368ff406ab12be8ab01ba81049f72448aed50deb..9463c06ee3dde8c1059e172021c5e3de52946248 100644 --- a/src/subscriber_diffdrive.cpp +++ b/src/subscriber_diffdrive.cpp @@ -12,13 +12,15 @@ namespace wolf { -SubscriberDiffdrive::SubscriberDiffdrive(const SensorBasePtr& sensor_ptr) - : Subscriber(sensor_ptr) +SubscriberDiffdrive::SubscriberDiffdrive(const std::string& _unique_name, + const ParamsServer& _server, + const SensorBasePtr _sensor_ptr) + : Subscriber(_unique_name, _server, _sensor_ptr) , last_odom_stamp_(ros::Time(0)) , last_odom_seq_(-1) { last_angles_ = Eigen::Vector2d(); - ticks_cov_factor_ = std::static_pointer_cast<SensorDiffDrive>(sensor_ptr)->getParams()->ticks_cov_factor; + ticks_cov_factor_ = std::static_pointer_cast<SensorDiffDrive>(_sensor_ptr)->getParams()->ticks_cov_factor; } void SubscriberDiffdrive::initSubscriber(ros::NodeHandle& nh, const std::string& topic) @@ -66,10 +68,4 @@ void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg) last_odom_seq_ = msg->header.seq; } -std::shared_ptr<Subscriber> SubscriberDiffdrive::create(const std::string& _unique_name, - const ParamsServer& _params, - const SensorBasePtr _sensor_ptr) -{ - return std::make_shared<SubscriberDiffdrive>(_sensor_ptr); -}; } // namespace wolf diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp index 6cebb0b94317a3294ba88d66c739edbe5852c122..90a02c628f936468127a8ee03b7fcfd585c78b9d 100644 --- a/src/subscriber_odom2d.cpp +++ b/src/subscriber_odom2d.cpp @@ -23,11 +23,13 @@ namespace wolf { -SubscriberOdom2d::SubscriberOdom2d(const SensorBasePtr& sensor_ptr) - : Subscriber(sensor_ptr) +SubscriberOdom2d::SubscriberOdom2d(const std::string& _unique_name, + const ParamsServer& _server, + const SensorBasePtr _sensor_ptr) + : Subscriber(_unique_name, _server, _sensor_ptr) , last_odom_stamp_(ros::Time(0)) - , odometry_translational_cov_factor_(std::static_pointer_cast<SensorOdom2d>(sensor_ptr)->getDispVarToDispNoiseFactor()) - , odometry_rotational_cov_factor_(std::static_pointer_cast<SensorOdom2d>(sensor_ptr)->getRotVarToRotNoiseFactor()) + , odometry_translational_cov_factor_(std::static_pointer_cast<SensorOdom2d>(_sensor_ptr)->getDispVarToDispNoiseFactor()) + , odometry_rotational_cov_factor_(std::static_pointer_cast<SensorOdom2d>(_sensor_ptr)->getRotVarToRotNoiseFactor()) { } @@ -58,10 +60,4 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg) ROS_DEBUG("WolfNodePolyline::odomCallback: end"); } -std::shared_ptr<Subscriber> SubscriberOdom2d::create(const std::string& _unique_name, - const ParamsServer& _params, - const SensorBasePtr _sensor_ptr) -{ - return std::make_shared<SubscriberOdom2d>(_sensor_ptr); -}; } // namespace wolf