storing loaders

include/node.h

@@ -79,6 +79,8 @@ class WolfRosNode
 
 
     protected:
+        std::vector<std::shared_ptr<Loader>> loaders_;
+
         // solver
         SolverManagerPtr solver_;
         ros::Time last_cov_stamp_;

include/publisher.h

@@ -97,6 +97,8 @@ class Publisher
 
         std::string getTopic() const;
 
+        std::string getName() const;
+
     protected:
 
         template<typename T>
@@ -129,6 +131,11 @@ inline std::string Publisher::getTopic() const
     return topic_;
 }
 
+inline std::string Publisher::getName() const
+{
+    return name_;
+}
+
 inline void Publisher::publish()
 {
     if (last_n_period_ == 0)

include/publisher_graph.h

@@ -99,8 +99,6 @@ class PublisherGraph: public Publisher
 
 };
 
-WOLF_REGISTER_PUBLISHER(PublisherGraph)
-
 }
 
 #endif

include/publisher_pose.h

@@ -78,7 +78,6 @@ class PublisherPose: public Publisher
         tf::TransformListener tfl_;
 };
 
-WOLF_REGISTER_PUBLISHER(PublisherPose)
 }
 
 #endif

include/publisher_state_block.h

@@ -59,7 +59,6 @@ class PublisherStateBlock: public Publisher
         void publishDerived() override;
 };
 
-WOLF_REGISTER_PUBLISHER(PublisherStateBlock)
 }
 
 #endif

include/publisher_tf.h

@@ -67,7 +67,7 @@ class PublisherTf: public Publisher
 
         tf::TransformListener tfl_;
 
-        tf::StampedTransform T_odom2base_;//, T_map2odom_;
+        tf::StampedTransform T_odom2base_;
         geometry_msgs::TransformStamped Tmsg_map2odom_;
 
         bool publish_odom_tf_;
@@ -86,7 +86,6 @@ class PublisherTf: public Publisher
         void publishDerived() override;
 };
 
-WOLF_REGISTER_PUBLISHER(PublisherTf)
 }
 
 #endif

include/publisher_trajectory.h

@@ -66,7 +66,6 @@ class PublisherTrajectory: public Publisher
 
 };
 
-WOLF_REGISTER_PUBLISHER(PublisherTrajectory)
 }
 
 #endif

include/subscriber_diffdrive.h

@@ -55,5 +55,4 @@ class SubscriberDiffdrive : public Subscriber
     void callback(const sensor_msgs::JointState::ConstPtr& msg);
 };
 
-WOLF_REGISTER_SUBSCRIBER(SubscriberDiffdrive)
 }  // namespace wolf

include/subscriber_odom2d.h

@@ -53,5 +53,4 @@ class SubscriberOdom2d : public Subscriber
     void callback(const nav_msgs::Odometry::ConstPtr& msg);
 };
 
-WOLF_REGISTER_SUBSCRIBER(SubscriberOdom2d)
 }  // namespace wolf

src/node.cpp

@@ -66,7 +66,6 @@ WolfRosNode::WolfRosNode()
     node_rate_ = server.getParam<double>("problem/node_rate");
 
     // LOAD PACKAGES (subscribers and publishers)
-    auto loaders = std::vector<std::shared_ptr<Loader>>();
 #if __APPLE__
     std::string lib_extension = ".dylib";
 #else
@@ -77,14 +76,14 @@ WolfRosNode::WolfRosNode()
         WOLF_TRACE("Loading subscriber " + subscriber_name + " via " + subscriber);
         auto l = std::make_shared<LoaderRaw>(subscriber);
         l->load();
-        loaders.push_back(l);
+        loaders_.push_back(l);
     }
     for (auto publisher_name : server.getParam<std::vector<std::string>>("packages_publisher")) {
         std::string publisher = packages_path + "/libpublisher_" + publisher_name + lib_extension;
         WOLF_TRACE("Loading publisher " + publisher_name + " via " + publisher);
         auto l = std::make_shared<LoaderRaw>(publisher);
         l->load();
-        loaders.push_back(l);
+        loaders_.push_back(l);
     }
     
     // PUBLISHERS
@@ -94,11 +93,11 @@ WolfRosNode::WolfRosNode()
         std::string publisher = it["type"];
         std::string topic      = it["topic"];
         WOLF_INFO("Pub: ", publisher, " name: ", publisher+" - "+topic);
-        publishers_.push_back(FactoryEmptyObject::create(publisher,
-                                                        publisher+" - "+topic,
-                                                        server,
-                                                        problem_ptr_,
-                                                        nh_));
+        publishers_.push_back(FactoryPublisher::create(publisher,
+                                                       publisher+" - "+topic,
+                                                       server,
+                                                       problem_ptr_,
+                                                       nh_));
     }
 
     // SUBSCRIBERS
@@ -268,7 +267,10 @@ int main(int argc, char **argv)
 
     // Init publishers threads
     for(auto pub : wolf_node.publishers_)
+    {
+        WOLF_INFO("Running publisher ", pub->getName());
         pub->run();
+    }
 
     while (ros::ok())
     {

src/publisher_graph.cpp

@@ -772,4 +772,6 @@ std::string PublisherGraph::factorString(FactorBaseConstPtr fac) const
     return factor_string;
 }
 
+WOLF_REGISTER_PUBLISHER(PublisherGraph)
+
 }

src/publisher_pose.cpp

@@ -263,4 +263,5 @@ bool PublisherPose::listenTf()
     return false;
 }
 
+WOLF_REGISTER_PUBLISHER(PublisherPose)
 }

src/publisher_state_block.cpp

@@ -64,4 +64,5 @@ void PublisherStateBlock::publishDerived()
 
 }
 
+WOLF_REGISTER_PUBLISHER(PublisherStateBlock)
 }

src/publisher_tf.cpp

@@ -47,10 +47,6 @@ PublisherTf::PublisherTf(const std::string& _unique_name,
     T_odom2base_.frame_id_ = odom_frame_id_;
     T_odom2base_.child_frame_id_ = base_frame_id_;
     T_odom2base_.stamp_ = ros::Time::now();
-    //T_map2odom_.setIdentity();
-    //T_map2odom_.frame_id_ = map_frame_id_;
-    //T_map2odom_.child_frame_id_ = odom_frame_id_;
-    //T_map2odom_.stamp_ = ros::Time::now();
     Tmsg_map2odom_.child_frame_id = odom_frame_id_;
     Tmsg_map2odom_.header.frame_id = map_frame_id_;
     Tmsg_map2odom_.header.stamp = ros::Time::now();
@@ -164,4 +160,5 @@ void PublisherTf::publishDerived()
     stfb_.sendTransform(Tmsg_map2odom_);
 }
 
+WOLF_REGISTER_PUBLISHER(PublisherTf)
 }

src/publisher_trajectory.cpp

@@ -106,4 +106,5 @@ void PublisherTrajectory::publishTrajectory()
     path_msg_.poses.clear();
 }
 
+WOLF_REGISTER_PUBLISHER(PublisherTrajectory)
 }

src/subscriber_diffdrive.cpp

@@ -88,4 +88,5 @@ void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg)
     last_odom_seq_   = msg->header.seq;
 }
 
+WOLF_REGISTER_SUBSCRIBER(SubscriberDiffdrive)
 }  // namespace wolf

src/subscriber_odom2d.cpp

@@ -81,4 +81,5 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg)
     ROS_DEBUG("WolfNodePolyline::odomCallback: end");
 }
 
+WOLF_REGISTER_SUBSCRIBER(SubscriberOdom2d)
 }  // namespace wolf