publisher of protection levels marker

CMakeLists.txt

@@ -137,7 +137,8 @@ include_directories(
 #   src/${PROJECT_NAME}/wolf_ros.cpp
 # )
 add_library(publisher_${PROJECT_NAME}
-  src/publisher_gnss_tf.cpp)
+  src/publisher_gnss_tf.cpp
+  src/publisher_gnss_accuracy.cpp)
 add_library(subscriber_${PROJECT_NAME}
   src/subscriber_gnss.cpp
   src/subscriber_gnss_fix.cpp

include/publisher_gnss_accuracy.h

+#ifndef PUBLISHER_GNSS_ACCURACY_H
+#define PUBLISHER_GNSS_ACCURACY_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+
+namespace wolf
+{
+
+class PublisherGnssAccuracy: public Publisher
+{
+        visualization_msgs::Marker PL_marker_msg_;
+        std_msgs::ColorRGBA marker_color_;
+        SensorBasePtr sensor_;
+        double k_H_, k_V_;
+
+    public:
+        PublisherGnssAccuracy(const std::string& _unique_name,
+                              const ParamsServer& _server,
+                              const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherGnssAccuracy);
+
+        virtual ~PublisherGnssAccuracy(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherGnssAccuracy)
+}
+
+#endif

src/publisher_gnss_accuracy.cpp

+#include "publisher_gnss_accuracy.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include "tf/transform_datatypes.h"
+
+namespace wolf
+{
+
+PublisherGnssAccuracy::PublisherGnssAccuracy(const std::string& _unique_name,
+                             const ParamsServer& _server,
+                             const ProblemPtr _problem) :
+        Publisher(_unique_name, _server, _problem)
+{
+    Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color");
+    marker_color_.r = col(0);
+    marker_color_.g = col(1);
+    marker_color_.b = col(2);
+    marker_color_.a = col(3);
+    sensor_ = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
+    k_H_    = _server.getParam<double>(prefix_ + "/k_H");
+    k_V_    = _server.getParam<double>(prefix_ + "/k_V");
+}
+
+void PublisherGnssAccuracy::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+    nh.param<std::string>("map_frame_id", PL_marker_msg_.header.frame_id, "map");
+
+    PL_marker_msg_.type = visualization_msgs::Marker::CYLINDER;
+    PL_marker_msg_.action = visualization_msgs::Marker::ADD;
+    PL_marker_msg_.ns = "PL";
+    PL_marker_msg_.color = marker_color_;
+    PL_marker_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+
+    publisher_ = nh.advertise<visualization_msgs::Marker>(topic, 1);
+}
+
+void PublisherGnssAccuracy::publishDerived()
+{
+    // CURRENT POSITION
+    VectorComposite current_state = problem_->getState("PO");
+    TimeStamp loc_ts = problem_->getTimeStamp();
+
+    // state not ready
+    if (current_state.count('P') == 0 or
+        current_state.count('O') == 0 or
+        not loc_ts.ok())
+    {
+        return;
+    }
+
+    // fill position of GNSS antenna
+    Eigen::Vector3d p = Eigen::Vector3d::Zero();
+    // 2D
+    if (problem_->getDim() == 2)
+        p.head(2) = current_state['P'] + Eigen::Rotation2Dd(current_state['O'](0)) * sensor_->getP()->getState().head(2);
+    // 3D
+    else
+        p = current_state['P'] + Eigen::Quaterniond(Eigen::Vector4d(current_state['O'])) * sensor_->getP()->getState();
+
+    // COVARIANCE
+    bool new_cov = false;
+    Eigen::MatrixXd cov(3,3);
+    // 2D
+    if (problem_->getDim() == 2)
+        throw std::runtime_error("not implemented");
+    // 3D
+    else
+    {
+        new_cov = problem_->getCovarianceBlock(problem_->getLastKeyFrame()->getP(),
+                                               problem_->getLastKeyFrame()->getP(),
+                                               cov, 0, 0);
+        WOLF_WARN_COND(not new_cov, "Last KF covariance could not be recovered, using the previous one");
+    }
+
+    // PROTECTION LEVELS
+    double HPL, VPL;
+    if (new_cov)
+    {
+        // Horizontal
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> es(cov.topLeftCorner<2,2>(), Eigen::EigenvaluesOnly);
+
+        if (es.info() == Eigen::Success)
+            HPL = k_H_ * sqrt(es.eigenvalues().maxCoeff());
+        else
+            new_cov = false;
+
+        WOLF_WARN_COND(not new_cov, "eigen decomposition failed! Cov horizontal:\n", cov.topLeftCorner<2,2>());
+
+        // Vertical
+        VPL = k_V_ * sqrt(cov(2,2));
+    }
+
+    // CYLINDER MARKER
+    PL_marker_msg_.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
+    PL_marker_msg_.pose.position.x = p(0);
+    PL_marker_msg_.pose.position.y = p(1);
+    PL_marker_msg_.pose.position.z = p(2);
+
+    if (new_cov)
+    {
+        PL_marker_msg_.scale.x = HPL;
+        PL_marker_msg_.scale.y = HPL;
+        PL_marker_msg_.scale.z = VPL;
+    }
+    publisher_.publish(PL_marker_msg_);
+}
+
+}