Added publish_landmarks_markers

cfg/AdcLandmarksSlamSolver.cfg

@@ -65,7 +65,7 @@ ceres.add("update_problem_angle",                 double_t,     0,           "An
 ceres.add("landmarks_map_file",                   str_t,        0,           "Input txt file path",                                                       "landmarks.txt")
 ceres.add("problem_frame_window",                 int_t,        0,           "Max number of frames to add",                                               30, -1, 120)
 
-flags.add("visualization",                        bool_t,       0,           "Boolean to publish visualization markers",                                  False)
+flags.add("publish_visualization",                bool_t,       0,           "Boolean to publish visualization markers",                                  False)
 flags.add("load_landmarks",                       bool_t,       0,           "Boolean to load_landmarks from txt file",                                   False)
 flags.add("landmarks_pos_fixed",                  bool_t,       0,           "Boolean to fix landmarks positions",                                        False)
 flags.add("search_for_new_landmarks",             bool_t,       0,           "Boolean to search for new landmarks",                                       False)

include/adc_landmarks_slam_solver_alg_node.h

@@ -210,6 +210,11 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
      */
     double mahalanobis_distance(Eigen::Vector2d feature_pos, double feature_r, double feature_th, Eigen::Vector2d landmark_pos);
 
+    /**
+     * \brief Function to publish landmarks markers.
+     */
+    void publish_landmarks_markers();
+
    /**
     * \brief config variable
     *

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -187,6 +187,8 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
     {
       std::cout << "new data" << std::endl;
       check_landmarks(this->config_.search_for_new_landmarks);
+      if (this->config_.publish_visualization)
+        publish_landmarks_markers();
       this->new_features_data = false;
     }
 
@@ -521,6 +523,45 @@ void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
   landmarks_file.close();
 }
 
+void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
+{
+  this->landmarks_MarkerArray_msg_.markers.resize(this->landmarks.size());
+  std::map<double, Landmark_info>::iterator it = this->landmarks.begin();
+  for (unsigned int i = 0; i < this->landmarks_MarkerArray_msg_.markers.size() && it != this->landmarks.end(); i++, ++it)
+  {
+    this->landmarks_MarkerArray_msg_.markers[i].header.frame_id = this->config_.global_frame;
+    this->landmarks_MarkerArray_msg_.markers[i].header.stamp = this->features_stamp;
+    this->landmarks_MarkerArray_msg_.markers[i].id = i;
+    this->landmarks_MarkerArray_msg_.markers[i].ns = "landmarks";
+    // this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::CYLINDER;
+    this->landmarks_MarkerArray_msg_.markers[i].action = visualization_msgs::Marker::ADD;
+    // this->landmarks_MarkerArray_msg_.markers[i].lifetime = ros::Duration(0.1);
+
+    this->landmarks_MarkerArray_msg_.markers[i].pose.position.x = it->second.pose(0);
+    this->landmarks_MarkerArray_msg_.markers[i].pose.position.y = it->second.pose(1);
+    this->landmarks_MarkerArray_msg_.markers[i].pose.position.z = 0.2;
+
+    this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.x = 0.0;
+    this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.y = 0.0;
+    this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.z = 0.0;
+    this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.w = 1.0;
+
+    this->landmarks_MarkerArray_msg_.markers[i].scale.x = 0.1;
+    this->landmarks_MarkerArray_msg_.markers[i].scale.y = 0.1;
+    this->landmarks_MarkerArray_msg_.markers[i].scale.z = 0.4;
+
+    this->landmarks_MarkerArray_msg_.markers[i].color.a = 1.0;
+    this->landmarks_MarkerArray_msg_.markers[i].color.r = (it->second.detected? 0.0: 1.0);
+    this->landmarks_MarkerArray_msg_.markers[i].color.g = 1.0;
+    this->landmarks_MarkerArray_msg_.markers[i].color.b = 1.0;
+
+    // this->landmarks_MarkerArray_msg_.markers[i].text = std::to_string(this->landmarks[i].intensity);
+  }
+  if (this->landmarks_MarkerArray_msg_.markers.size() > 0)
+    this->landmarks_publisher_.publish(this->landmarks_MarkerArray_msg_);
+}
+
 /*  [subscriber callbacks] */
 void AdcLandmarksSlamSolverAlgNode::estimated_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
 {