Draft: Resolve "add_falko_publisher"

Closes #4 (closed)

include/publisher_falko_bsc.h

+#ifndef PUBLISHER_FALKO_BSC_H
+#define PUBLISHER_FALKO__BSC_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <tf/transform_listener.h>
+#include <sensor_msgs/LaserScan.h>
+#include <visualization_msgs/MarkerArray.h>
+
+/**************************
+ *      laserscanutils includes      *
+ **************************/
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_scene_falko.h"
+
+namespace wolf {
+
+class PublisherFalkoBsc : public Publisher
+{
+    bool pose_array_, marker_, pose_with_cov_;
+    int  max_points_;
+
+    visualization_msgs::Marker marker_msg_falko_scene;
+
+    std_msgs::ColorRGBA marker_color_target_;
+    std_msgs::ColorRGBA marker_color_reference_;
+
+    bool          extrinsics_;
+    SensorBasePtr sensor_;
+    std::string   frame_id_, map_frame_id_;
+
+    FrameBasePtr last_frame;
+    FrameBasePtr KF_old;
+
+    ros::Publisher pub_keypoints_;
+    ros::Publisher pub_marker_scene_target_;
+    ros::Publisher pub_marker_scene_reference_;
+    ros::Publisher pub_marker_associations_;
+
+    // Reference scene info
+    std::map<CaptureBasePtr, visualization_msgs::Marker> map_markers_reference_scenes_;
+
+  public:
+    PublisherFalkoBsc(const std::string &_unique_name, const ParamsServer &_server, const ProblemPtr _problem);
+    WOLF_PUBLISHER_CREATE(PublisherFalkoBsc);
+
+    virtual ~PublisherFalkoBsc(){};
+
+    void initialize(ros::NodeHandle &nh, const std::string &topic) override;
+
+    void publishDerived() override;
+
+    void publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
+                      Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
+                      std_msgs::ColorRGBA _marker_color, int & _id, CaptureBasePtr cap);
+
+    void publishEmpty();
+
+    void publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
+                             CaptureBasePtr &cap_reference);
+
+    void getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
+                             std::vector<falkolib::FALKO> &_keypoints);
+
+    void getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite current_state);
+
+  protected:
+    bool                  listenTf();
+    Eigen::Quaterniond    q_frame_;
+    Eigen::Vector3d       t_frame_;
+    tf::TransformListener tfl_;
+    int                   id_old_ = 1;
+
+    std::map<FrameBasePtr, VectorComposite> map_frame_states;
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherFalkoBsc)
+} // namespace wolf
+
+#endif