From 6db0da3f9165f41e4f6323b0dcecf20798749827 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Wed, 30 Mar 2022 12:18:22 +0200
Subject: [PATCH] storing loaders
---
include/node.h | 2 ++
include/publisher.h | 7 +++++++
include/publisher_graph.h | 2 --
include/publisher_pose.h | 1 -
include/publisher_state_block.h | 1 -
include/publisher_tf.h | 3 +--
include/publisher_trajectory.h | 1 -
include/subscriber_diffdrive.h | 1 -
include/subscriber_odom2d.h | 1 -
src/node.cpp | 18 ++++++++++--------
src/publisher_graph.cpp | 2 ++
src/publisher_pose.cpp | 1 +
src/publisher_state_block.cpp | 1 +
src/publisher_tf.cpp | 5 +----
src/publisher_trajectory.cpp | 1 +
src/subscriber_diffdrive.cpp | 1 +
src/subscriber_odom2d.cpp | 1 +
17 files changed, 28 insertions(+), 21 deletions(-)
diff --git a/include/node.h b/include/node.h
index 3113944..0af71ac 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 89781a4..822b2e4 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 fec1e5f..de5d2e4 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 d6c72de..6005454 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 92ffb5d..053dc40 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 c9105d2..4b4eff9 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 eddfa60..53f3340 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 935d729..3638e5b 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 c172d28..db786e7 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 16f4591..1b78215 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 2c9373f..19ba360 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 d1ba471..7c85dc1 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 1bf55b3..bc39947 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 08117d5..044f820 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 6fa3576..e220449 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 a7578d0..da7a295 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 2438e41..f46d0a5 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
--
GitLab