diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index a626833c99d6b5e6987fab4259f68bf466bd6570..567c739200d7effaece6fe95cc6f17ce36f50c0c 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -42,7 +42,7 @@ class ProcessorOdom2d : public ProcessorMotion
public:
ProcessorOdom2d(ParamsProcessorOdom2dPtr _params);
virtual ~ProcessorOdom2d();
- virtual void configure(SensorBasePtr _sensor) override;
+ virtual void configure(SensorBasePtr _sensor) override { };
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ParamsProcessorOdom2d);
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
index 1b6f659ebd609dffef4f3c472800cf32714f22e1..1e11cb86aeb89932815e38708f3cbf36530e2006 100644
--- a/include/core/yaml/parser_yaml.h
+++ b/include/core/yaml/parser_yaml.h
@@ -30,6 +30,7 @@ class ParserYAML {
YAML::Node n_;
};
struct PublisherManager{
+ std::string package_;
std::string subscriber_;
std::string topic_;
std::string period_;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 9e63f71a1b2fa1a257c209cee391ab84f352a264..7acce5acb13de2ec58e52f76197932119e7a86a1 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -134,6 +134,13 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
l->load();
loaders.push_back(l);
}
+ for (auto it : _server.getParam<std::vector<std::string>>("packages")) {
+ std::string subscriber = packages_path + "/libpublisher_" + it + lib_extension;
+ WOLF_TRACE("Loading publisher " + subscriber);
+ auto l = new LoaderRaw(subscriber);
+ l->load();
+ loaders.push_back(l);
+ }
std::vector<std::string> raw_libs;
try {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 1bd0ca0ef6cacb1c90d0a7cdf843593bb38ac5ab..6993332c3e23dec79c7697abe6f56a1feb531ce7 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -36,8 +36,7 @@ void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
assert(_sensor && "Provided sensor is nullptr");
- SensorDiffDriveConstPtr sensor_diff_drive = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor);
- assert(sensor_diff_drive != nullptr && "Sensor is not of type SensorDiffDrive");
+ SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 6945b82ea7391816cd4cf924b42d682662263cf9..bc11469c5c8aa5a5bb4c26c5a41f632c3f89312f 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -90,7 +90,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
return;
}
- incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+ incoming_ptr_ = std::dynamic_pointer_cast<CaptureMotion>(_incoming_ptr);
+ assert(incoming_ptr_ != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureMotion").c_str());
preProcess(); // Derived class operations
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 4a126d901eb291a9d5fc8537b5429b294bfdc025..04265e964c385bdbe591e84c650f8c7d99c36eeb 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -16,11 +16,6 @@ ProcessorOdom2d::~ProcessorOdom2d()
{
}
-void ProcessorOdom2d::configure(SensorBasePtr _sensor)
-{
- auto sensor_ = std::dynamic_pointer_cast<SensorOdom2d>(_sensor);
- assert(sensor_ != nullptr && "Sensor is not of type SensorOdom2d");
-}
void ProcessorOdom2d::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib, const double _dt, Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _jacobian_calib) const
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 71c77e1e16f9cc3b5b5b76885b64cd67b891bb7b..4bce5f885a2104ee45c646e3a2a4e45a9d840b49 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -21,8 +21,8 @@ ProcessorOdom3d::~ProcessorOdom3d()
void ProcessorOdom3d::configure(SensorBasePtr _sensor)
{
assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
- auto sen_ptr = std::dynamic_pointer_cast<SensorOdom3d>(_sensor);
- assert(sen_ptr != nullptr && "Sensor is not of type SensorOdom3d");
+
+ SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor);
// we steal the parameters from the provided odom3d sensor.
k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
@@ -151,7 +151,7 @@ void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
Eigen::VectorXd& _x_plus_delta) const
-{
+{
assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index 267d0c9da2024abd59ed5b529d13de7680d14472..f2dd93e7443c78f7714e15c567ce7980b3406be2 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.cpp
@@ -430,12 +430,12 @@ void ParserYAML::parseFirstLevel(std::string _file)
{
for (const auto& kv : n_config["ROS publisher"])
{
- WOLF_DEBUG("WHAT");
PublisherManager pPublisherManager = {
- kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
+ kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
};
publisher_managers_.push_back(pPublisherManager);
- map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ map_container.push_back(std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+ { "type", kv["type"].Scalar() },
{ "topic", kv["topic"].Scalar() },
{ "period", kv["period"].Scalar() } }));
}
@@ -445,7 +445,7 @@ void ParserYAML::parseFirstLevel(std::string _file)
catch (YAML::InvalidNode& e)
{
throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
- ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
+ ". Please make sure that each manager has 'package', 'type', 'topic' and 'period' entries.");
}
}
std::map<std::string, std::string> ParserYAML::getParams()
@@ -482,6 +482,8 @@ void ParserYAML::parse()
plugins.push_back(it.plugin_);
for (const auto& it : subscriber_managers_)
packages.push_back(it.package_);
+ for (const auto& it : publisher_managers_)
+ packages.push_back(it.package_);
plugins.sort();
plugins.unique();
packages.sort();