diff --git a/include/publisher_gnss_accuracy.h b/include/publisher_gnss_accuracy.h
index 57ca6cfb27e97a50d925f07cb779f29fac88352e..c494fc4da4d796de4d0741153d14b339e05d1cac 100644
--- a/include/publisher_gnss_accuracy.h
+++ b/include/publisher_gnss_accuracy.h
@@ -42,13 +42,13 @@ class PublisherGnssAccuracy: public Publisher
{
visualization_msgs::Marker PL_marker_msg_;
std_msgs::ColorRGBA marker_color_;
- SensorBasePtr sensor_;
+ SensorBaseConstPtr sensor_;
double k_H_, k_V_;
public:
PublisherGnssAccuracy(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem);
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherGnssAccuracy);
virtual ~PublisherGnssAccuracy(){};
diff --git a/include/publisher_gnss_fix.h b/include/publisher_gnss_fix.h
index a4f0caf70dcbfe8f693342c7d5651cc067962e10..4f2daad7ad59c5a6ef3619333edb5c9c370a7e8d 100644
--- a/include/publisher_gnss_fix.h
+++ b/include/publisher_gnss_fix.h
@@ -45,15 +45,15 @@ class PublishGnssFix: public Publisher
geometry_msgs::PoseArray pose_array_msg_;
geometry_msgs::PoseStamped pose_msg_;
visualization_msgs::Marker marker_msg_;
- SensorBasePtr sensor_;
+ SensorBaseConstPtr sensor_;
std::string frame_id_, map_frame_id_;
ros::Publisher pub_pose_array_, pub_marker_, pub_pose_;
public:
PublishGnssFix(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem);
+ const ParamsServer& _server,
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublishGnssFix);
virtual ~PublishGnssFix(){};
diff --git a/include/publisher_gnss_tf.h b/include/publisher_gnss_tf.h
index 6779049080bb5e17a560cb611db673b48fd42158..6d49cbe48b16e8df87390ab2004cfd5c659c8570 100644
--- a/include/publisher_gnss_tf.h
+++ b/include/publisher_gnss_tf.h
@@ -40,12 +40,12 @@ class PublisherGnssTf: public Publisher
std::string map_frame_id_, enu_frame_id_, ecef_frame_id_;
tf::StampedTransform T_enu_map_, T_ecef_enu_;
tf::TransformBroadcaster tfb_;
- SensorGnssPtr sensor_gnss_;
+ SensorGnssConstPtr sensor_gnss_;
public:
PublisherGnssTf(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem);
+ const ParamsServer& _server,
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherGnssTf);
virtual ~PublisherGnssTf(){};
diff --git a/include/publisher_tracker_gnss_info.h b/include/publisher_tracker_gnss_info.h
index ce057285292583562c24510723ede5976093d589..19ccd90f25f51565bf2121860e73188f1e597dff 100644
--- a/include/publisher_tracker_gnss_info.h
+++ b/include/publisher_tracker_gnss_info.h
@@ -42,13 +42,13 @@ class PublisherTrackerGnssInfo: public Publisher
{
protected:
- ProcessorTrackerGnssPtr processor_tracker_gnss_;
+ ProcessorTrackerGnssConstPtr processor_tracker_gnss_;
ros::Publisher publisher_untracked_;
public:
PublisherTrackerGnssInfo(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem);
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherTrackerGnssInfo);
virtual ~PublisherTrackerGnssInfo(){};
diff --git a/src/publisher_gnss_accuracy.cpp b/src/publisher_gnss_accuracy.cpp
index da18a60dff6ffd1fb077dd6b073cadef379cdd92..f57141022e54f96e5cd05d9e6134cebd44444f6f 100644
--- a/src/publisher_gnss_accuracy.cpp
+++ b/src/publisher_gnss_accuracy.cpp
@@ -31,9 +31,9 @@ namespace wolf
{
PublisherGnssAccuracy::PublisherGnssAccuracy(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem) :
- Publisher(_unique_name, _server, _problem)
+ const ParamsServer& _server,
+ ProblemConstPtr _problem) :
+ Publisher(_unique_name, _server, _problem)
{
Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color");
marker_color_.r = col(0);
diff --git a/src/publisher_gnss_fix.cpp b/src/publisher_gnss_fix.cpp
index 814e87091e7275a77f9eeaa9e554015af26bac54..5b4750ca16e3139ff16258d001cccb3724eff3cd 100644
--- a/src/publisher_gnss_fix.cpp
+++ b/src/publisher_gnss_fix.cpp
@@ -28,8 +28,8 @@ namespace wolf
{
PublishGnssFix::PublishGnssFix(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem) :
+ const ParamsServer& _server,
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
sensor_ = _problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
diff --git a/src/publisher_gnss_tf.cpp b/src/publisher_gnss_tf.cpp
index 9ec259e2dca3067f12a8a5aae5653c3f6776a48b..da778fbb3e6ff5cae76a2b4c9a12ccf50e824611 100644
--- a/src/publisher_gnss_tf.cpp
+++ b/src/publisher_gnss_tf.cpp
@@ -33,10 +33,10 @@ namespace wolf
PublisherGnssTf::PublisherGnssTf(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem) :
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
- sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name")));
+ sensor_gnss_ = std::static_pointer_cast<const SensorGnss>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name")));
T_enu_map_.setIdentity();
T_enu_map_.frame_id_ = "ENU";
diff --git a/src/publisher_tracker_gnss_info.cpp b/src/publisher_tracker_gnss_info.cpp
index 1fc154f2a2390adf28538b365294d3654d00632d..4634a96cd59bc5d1d88eaf25e148ac9c969a439c 100644
--- a/src/publisher_tracker_gnss_info.cpp
+++ b/src/publisher_tracker_gnss_info.cpp
@@ -29,24 +29,13 @@ namespace wolf
PublisherTrackerGnssInfo::PublisherTrackerGnssInfo(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem) :
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem),
processor_tracker_gnss_(nullptr)
{
auto proc_name = _server.getParam<std::string>(prefix_ + "/processor_gnss");
- for (auto sen : _problem->getHardware()->getSensorList())
- {
- for (auto proc : sen->getProcessorList())
- if (proc_name == proc->getName())
- {
- processor_tracker_gnss_ = std::dynamic_pointer_cast<ProcessorTrackerGnss>(proc);
- if (not processor_tracker_gnss_)
- throw std::runtime_error("PublisherTrackerGnssInfo: the processor with name " + proc_name + " is not of type ProcessorTrackerGnss.");
- break;
- }
- if (processor_tracker_gnss_)
- break;
- }
+ processor_tracker_gnss_ = std::dynamic_pointer_cast<const ProcessorTrackerGnss>(problem_->findProcessor(proc_name));
+
if (not processor_tracker_gnss_)
throw std::runtime_error("PublisherTrackerGnssInfo: the processor with name " + proc_name + " was not found.");
}