diff --git a/README.md b/README.md
index ecdca06f56f60bda36ceb2e94a535cc5b85ec6cf..c8423d23f45108e0ed3ff1e75c6cec146a67166c 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,3 @@
-# wolf_ros_node
+# WOLF ROS node
-Wolf ROS node
+For installation guide and code documentation, please visit the [documentation website](http://www.iri.upc.edu/wolf).
diff --git a/include/factory_publisher.h b/include/factory_publisher.h
index d70f70705f07a83998caac6ceb86060d7110b2dd..79f2d1b86bfde30b0d3282f06a28a3b209c0bde6 100644
--- a/include/factory_publisher.h
+++ b/include/factory_publisher.h
@@ -26,11 +26,6 @@
#include <core/common/factory.h>
#include <core/utils/params_server.h>
#include <ros/ros.h>
-#include <nav_msgs/Odometry.h>
-
-// #include "wolf_ros_subscriber.h"
-// std
-
namespace wolf
{
diff --git a/include/factory_subscriber.h b/include/factory_subscriber.h
index ec0f17f948f98cc5aea5916cf71d0f77489cd288..dc23500bc1842bcb79acec461f72603dbba1ded8 100644
--- a/include/factory_subscriber.h
+++ b/include/factory_subscriber.h
@@ -26,7 +26,6 @@
#include <core/common/factory.h>
#include <core/utils/params_server.h>
#include <ros/ros.h>
-#include <nav_msgs/Odometry.h>
// #include "wolf_ros_subscriber.h"
// std
@@ -177,12 +176,12 @@ namespace wolf
*
* You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
*/
- class Subscriber;
- typedef Factory<Subscriber,
- const std::string&,
- const ParamsServer&,
- const SensorBasePtr,
- ros::NodeHandle&> FactorySubscriber;
+class Subscriber;
+typedef Factory<Subscriber,
+ const std::string&,
+ const ParamsServer&,
+ const SensorBasePtr,
+ ros::NodeHandle&> FactorySubscriber;
template<>
inline std::string FactorySubscriber::getClass() const
{
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.h b/include/subscriber.h
index 950929b7f116825c82c6d2e2e15ce283cbb4e15f..806a834dcf139a288383d44d0612ab3512b04e6d 100644
--- a/include/subscriber.h
+++ b/include/subscriber.h
@@ -25,6 +25,8 @@
/**************************
* WOLF includes *
**************************/
+#include "core/common/wolf.h"
+#include <core/utils/params_server.h>
#include <core/sensor/sensor_base.h>
#include "factory_subscriber.h"
@@ -32,6 +34,7 @@
* ROS includes *
**************************/
#include <ros/ros.h>
+#include <std_msgs/Header.h>
namespace wolf {
WOLF_PTR_TYPEDEFS(Subscriber);
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 d51fd73e76e46507f08db6d17aac04343ec83d9e..1b78215f1d2065fc4032fe87f04837f7f1016201 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -32,7 +32,7 @@ WolfRosNode::WolfRosNode()
, last_print_(ros::Time(0))
{
// ROS PARAMS
- std::string yaml_file, plugins_path, subscribers_path;
+ std::string yaml_file, plugins_path, packages_path;
nh_.param<std::string>("yaml_file_path",
yaml_file,
ros::package::getPath("wolf_ros_node") + "/yaml/params_demo.yaml");
@@ -40,7 +40,7 @@ WolfRosNode::WolfRosNode()
plugins_path,
"/usr/local/lib/");
nh_.param<std::string>("packages_path",
- subscribers_path,
+ packages_path,
ros::package::getPath("wolf_ros_node") + "/../../devel/lib/");
// PARAM SERVER CONFIGURATION
@@ -52,8 +52,6 @@ WolfRosNode::WolfRosNode()
ParamsServer server = ParamsServer(parser.getParams());
server.addParam("plugins_path", plugins_path);
- server.addParam("packages_path", subscribers_path);
-
server.print();
// PROBLEM
@@ -67,6 +65,41 @@ WolfRosNode::WolfRosNode()
// ROS
node_rate_ = server.getParam<double>("problem/node_rate");
+ // LOAD PACKAGES (subscribers and publishers)
+#if __APPLE__
+ std::string lib_extension = ".dylib";
+#else
+ std::string lib_extension = ".so";
+#endif
+ for (auto subscriber_name : server.getParam<std::vector<std::string>>("packages_subscriber")) {
+ std::string subscriber = packages_path + "/libsubscriber_" + subscriber_name + lib_extension;
+ WOLF_TRACE("Loading subscriber " + subscriber_name + " via " + subscriber);
+ auto l = std::make_shared<LoaderRaw>(subscriber);
+ l->load();
+ 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);
+ }
+
+ // PUBLISHERS
+ ROS_INFO("Creating publishers...");
+ for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS publisher"))
+ {
+ std::string publisher = it["type"];
+ std::string topic = it["topic"];
+ WOLF_INFO("Pub: ", publisher, " name: ", publisher+" - "+topic);
+ publishers_.push_back(FactoryPublisher::create(publisher,
+ publisher+" - "+topic,
+ server,
+ problem_ptr_,
+ nh_));
+ }
+
// SUBSCRIBERS
ROS_INFO("Creating subscribers...");
for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS subscriber"))
@@ -82,18 +115,6 @@ WolfRosNode::WolfRosNode()
nh_));
}
- // PUBLISHERS
- ROS_INFO("Creating publishers...");
- for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS publisher"))
- {
- WOLF_INFO("Pub: ", it["type"]);
- publishers_.push_back(FactoryPublisher::create(it["type"],
- it["type"]+" - "+it["topic"],
- server,
- problem_ptr_,
- nh_));
- }
-
// PROFILING
profiling_ = server.getParam<bool>("debug/profiling");
if (profiling_)
@@ -246,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..af32b73f629f80bc63b20947cfbf1482086912c2 100644
--- a/src/publisher_tf.cpp
+++ b/src/publisher_tf.cpp
@@ -47,10 +47,13 @@ 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();
+<<<<<<< HEAD
//T_map2odom_.setIdentity();
//T_map2odom_.frame_id_ = map_frame_id_;
//T_map2odom_.child_frame_id_ = odom_frame_id_;
//T_map2odom_.stamp_ = ros::Time::now();
+=======
+>>>>>>> devel
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 +167,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