Merge remote-tracking branch 'origin/devel' into 1-adapt-to-core-cmake-refactor

.gitignore

+/.cproject
+/.project
+/.settings/language.settings.xml

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(){};

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(){};

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(){};

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(){};

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);

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"));

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";

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.");
 }