diff --git a/include/node.h b/include/node.h index 3113944caafbe85c386ed2454e7b4bf014f31d65..0af71ac04df2154ae3a9ee4e4428b4b201d14388 100644 --- a/include/node.h +++ b/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_; diff --git a/include/publisher.h b/include/publisher.h index 89781a49ec982cb193530827b0c8b5cddd79b1c0..822b2e4158266ed1f0280cd295b4ea639933c362 100644 --- a/include/publisher.h +++ b/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) diff --git a/include/publisher_graph.h b/include/publisher_graph.h index fec1e5f8940b175233bec229b5212543f7a98af6..de5d2e4c966526dd6c435dd5f2c7308149c7b11d 100644 --- a/include/publisher_graph.h +++ b/include/publisher_graph.h @@ -99,8 +99,6 @@ class PublisherGraph: public Publisher }; -WOLF_REGISTER_PUBLISHER(PublisherGraph) - } #endif diff --git a/include/publisher_pose.h b/include/publisher_pose.h index d6c72de08aadf1d6c860581367c04d8298ddc0e5..6005454986066cc185241ccd159fba280903fc50 100644 --- a/include/publisher_pose.h +++ b/include/publisher_pose.h @@ -78,7 +78,6 @@ class PublisherPose: public Publisher tf::TransformListener tfl_; }; -WOLF_REGISTER_PUBLISHER(PublisherPose) } #endif diff --git a/include/publisher_state_block.h b/include/publisher_state_block.h index 92ffb5df49ac0e75cc792b5725700811719c525c..053dc40b7f22fbfbaf8e0834e105cc3baf4a0e18 100644 --- a/include/publisher_state_block.h +++ b/include/publisher_state_block.h @@ -59,7 +59,6 @@ class PublisherStateBlock: public Publisher void publishDerived() override; }; -WOLF_REGISTER_PUBLISHER(PublisherStateBlock) } #endif diff --git a/include/publisher_tf.h b/include/publisher_tf.h index c9105d2bc9a86d4d69ac06775a97e30cca0ba740..4b4eff9d35c3051a1dec6417106337519e4ba2a0 100644 --- a/include/publisher_tf.h +++ b/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 diff --git a/include/publisher_trajectory.h b/include/publisher_trajectory.h index eddfa6073ba7bbdff9f92f34f395173e10311aaa..53f3340a1519fa7fc31df1df78cd5df0aed2ad0a 100644 --- a/include/publisher_trajectory.h +++ b/include/publisher_trajectory.h @@ -66,7 +66,6 @@ class PublisherTrajectory: public Publisher }; -WOLF_REGISTER_PUBLISHER(PublisherTrajectory) } #endif diff --git a/include/subscriber_diffdrive.h b/include/subscriber_diffdrive.h index 935d729b4e4a08fb13abc8d284198765ea7bfaa7..3638e5b372884b07d3398d78fb0d4eb3bf8f172a 100644 --- a/include/subscriber_diffdrive.h +++ b/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 diff --git a/include/subscriber_odom2d.h b/include/subscriber_odom2d.h index c172d281b0d90613c674cd3ac6a5223c2c3507c7..db786e723b76f644efa934c8eeda30fcd7fcdb45 100644 --- a/include/subscriber_odom2d.h +++ b/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 diff --git a/src/node.cpp b/src/node.cpp index 16f4591e153d5eb30c3aec284b8d794d1e4f41ef..1b78215f1d2065fc4032fe87f04837f7f1016201 100644 --- a/src/node.cpp +++ b/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()) { diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp index 2c9373f876127ea0408df8127f759f209c530557..19ba3608f858726e99dc12ebe9bf705ef37d5ba9 100644 --- a/src/publisher_graph.cpp +++ b/src/publisher_graph.cpp @@ -772,4 +772,6 @@ std::string PublisherGraph::factorString(FactorBaseConstPtr fac) const return factor_string; } +WOLF_REGISTER_PUBLISHER(PublisherGraph) + } diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp index d1ba47180c2800e2b9c71e958b51ea59ad53677a..7c85dc12794aa3b7111892328b55b5820258dd64 100644 --- a/src/publisher_pose.cpp +++ b/src/publisher_pose.cpp @@ -263,4 +263,5 @@ bool PublisherPose::listenTf() return false; } +WOLF_REGISTER_PUBLISHER(PublisherPose) } diff --git a/src/publisher_state_block.cpp b/src/publisher_state_block.cpp index 1bf55b37f64b17e89bfe149f0d36f53694412382..bc39947dab7bd5d0691e99505b4916da308ee32b 100644 --- a/src/publisher_state_block.cpp +++ b/src/publisher_state_block.cpp @@ -64,4 +64,5 @@ void PublisherStateBlock::publishDerived() } +WOLF_REGISTER_PUBLISHER(PublisherStateBlock) } diff --git a/src/publisher_tf.cpp b/src/publisher_tf.cpp index 08117d5f6f2a91cfdc9a292f99969f5718300ff6..044f820a1646195c3a9406607c35eb1e90de00a3 100644 --- a/src/publisher_tf.cpp +++ b/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) } diff --git a/src/publisher_trajectory.cpp b/src/publisher_trajectory.cpp index 6fa357685b30859115a70d7bbb1c213402e27e1e..e220449a651bab3f8efd2876ffd1e45adbaeafee 100644 --- a/src/publisher_trajectory.cpp +++ b/src/publisher_trajectory.cpp @@ -106,4 +106,5 @@ void PublisherTrajectory::publishTrajectory() path_msg_.poses.clear(); } +WOLF_REGISTER_PUBLISHER(PublisherTrajectory) } diff --git a/src/subscriber_diffdrive.cpp b/src/subscriber_diffdrive.cpp index a7578d05ebd5220c5daf773283e8d943b4b9f065..da7a29504cadb1209c504a26892f1d443681d462 100644 --- a/src/subscriber_diffdrive.cpp +++ b/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 diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp index 2438e41d73e4d3558c0b4955e69354015a18b122..f46d0a5e2eb8623bd16845d86f3dca1fb90f051f 100644 --- a/src/subscriber_odom2d.cpp +++ b/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