Added xodr parser to load landmarks

CMakeLists.txt

 cmake_minimum_required(VERSION 2.8.3)
 project(iri_adc_landmarks_slam_solver)
 
+ADD_DEFINITIONS(-D_HAVE_XSD)
+
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED)
 # ******************************************************************** 
@@ -12,6 +14,9 @@ find_package(catkin REQUIRED COMPONENTS iri_base_algorithm visualization_msgs ge
 # find_package(Boost REQUIRED COMPONENTS system)
 find_package(Eigen3 REQUIRED)
 find_package(Ceres REQUIRED)
+find_package(iriutils REQUIRED)
+find_package(autonomous_driving_tools REQUIRED)
+find_package(opendrive_road_map REQUIRED)
 
 # ******************************************************************** 
 #           Add system and labrobotica dependencies here
@@ -80,6 +85,9 @@ include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
 include_directories(${EIGEN3_INCLUDE_DIR})
 include_directories(${CERES_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIRS})
+include_directories(${autonomous_driving_tools_INCLUDE_DIRS})
+include_directories(${opendrive_road_map_INCLUDE_DIRS})
 # include_directories(${<dependency>_INCLUDE_DIRS})
 
 ## Declare a cpp library
@@ -93,6 +101,9 @@ add_executable(${PROJECT_NAME} src/adc_landmarks_slam_solver_alg.cpp src/adc_lan
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${autonomous_driving_tools_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${opendrive_road_map_LIBRARIES})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARIES})
 
 # ******************************************************************** 

cfg/AdcLandmarksSlamSolver.cfg

@@ -70,6 +70,7 @@ ceres.add("update_problem_angle",                 double_t,     0,           "An
 ceres.add("update_problem_features_detected",     int_t,        0,           "Update problem when a lot of features are detected",                        3,   1, 10)
 ceres.add("wait_feature_detected_timeout",        double_t,     0,           "Timeout on a update problem event to wait for a feature detection",         0.2,   0.01, 0.5)
 ceres.add("landmarks_map_file",                   str_t,        0,           "Input txt file path",                                                       "landmarks.txt")
+ceres.add("load_landmarks_from_xodr",             bool_t,       0,           "If the landmarks map file is an xodr",                                      False)
 ceres.add("problem_frame_window",                 int_t,        0,           "Max number of frames to add",                                               30, -1, 500)
 
 flags.add("publish_visualization",                bool_t,       0,           "Boolean to publish visualization markers",                                  False)

include/adc_landmarks_slam_solver_alg.h

@@ -42,6 +42,17 @@
 #include "landmark_residual_from_base_2D.h"
 #include "sensor_pose_residual_2D.h"
 
+#include <cmath>
+
+#define SIGN_MARKER_ID 0
+#define SEMAPHORE_TYPE "1000001"
+#define SEMAPHORE_MARKER_ID 1
+#define LOC_SIGN_TRIANGLE_TYPE ""
+#define LOC_SIGN_TRIANGLE_MARKER_ID 2
+#define LOC_SIGN_ID_OFFSET 100
+#define LOC_SIGN_TRIANGLE_WIDTH 0.074
+#define LOC_SIGN_TRIANGLE_ANGLE M_PI/2
+
 //include adc_landmarks_slam_solver_alg main library
 
 /**
@@ -117,11 +128,24 @@ class AdcLandmarksSlamSolverAlgorithm
      *
      * \param _landmarks_file The route to the landmarks file to be loaded.
      * 
-     * \param _fixed If landamrks are fixed.
+     * \param _fixed If landmarks are fixed.
+     * 
+     * \param _from_xodr If landamrks are loaded form an xodr file.
+     *
+     * \return If success.
+     */
+    bool load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr);
+
+    /**
+     * \brief Function to load landmarks map from a xodr file.
+     *
+     * \param _landmarks_file The route to the landmarks file to be loaded.
+     * 
+     * \param _fixed If landmarks are fixed.
      *
      * \return If success.
      */
-    bool load_landmarks(const std::string& _landmarks_file, bool _fixed);
+    bool load_from_xodr(const std::string& _landmarks_file, bool _fixed);
 
     /**
      * \brief Function to add a landmark.

src/adc_landmarks_slam_solver_alg.cpp

 #include "adc_landmarks_slam_solver_alg.h"
+#include "opendrive_road.h"
+#include "exceptions.h"
+#include "opendrive_road_segment.h"
+#include "opendrive_signal.h"
 
 AdcLandmarksSlamSolverAlgorithm::AdcLandmarksSlamSolverAlgorithm(void)
 {
@@ -51,8 +55,10 @@ void AdcLandmarksSlamSolverAlgorithm::config_update(Config& config, uint32_t lev
 }
 
 // AdcLandmarksSlamSolverAlgorithm Public API
-bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file, bool _fixed)
+bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr)
 {
+  if (_from_xodr)
+    return load_from_xodr(_landmarks_file, _fixed);
   std::ifstream landmarks_file;
   landmarks_file.open(_landmarks_file.c_str());
   if (!landmarks_file.good())
@@ -79,6 +85,76 @@ bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmar
   return true;
 }
 
+bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmarks_file, bool _fixed)
+{
+  COpendriveRoad road;
+  std::string type, subtype;
+  TOpendriveWorldPose world;
+  try{
+    road.load(_landmarks_file);
+    std::map<double, LandmarkInfo>().swap(this->landmarks_);
+
+    for(unsigned int i=0;i<road.get_num_segments();i++)
+    {
+      const COpendriveRoadSegment &segment=road.get_segment(i);
+      for(unsigned int j=0;j<segment.get_num_signals();j++)
+      {
+        LandmarkInfo l;
+        l.detected = false;
+        l.source_frame_id = "";
+        l.fixed = _fixed;
+
+        const COpendriveSignal &sign = segment.get_signal(j);
+        sign.get_type(type,subtype);
+        if (type == LOC_SIGN_TRIANGLE_TYPE)
+        {
+          double loc_x, loc_y, loc_yaw, id;
+
+          l.type = LOC_SIGN_TRIANGLE_MARKER_ID;
+
+          world = sign.get_world_pose();
+          loc_x = -LOC_SIGN_TRIANGLE_WIDTH*std::sin(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
+          loc_y = -LOC_SIGN_TRIANGLE_WIDTH*std::cos(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
+          loc_yaw = M_PI + LOC_SIGN_TRIANGLE_ANGLE/2;
+          l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
+          l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
+          l.pose(2) = loc_yaw + world.heading;
+
+          id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2-1;
+          this->landmarks_[id] = l;
+
+          loc_x = LOC_SIGN_TRIANGLE_WIDTH*std::sin(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
+          loc_y = -LOC_SIGN_TRIANGLE_WIDTH*std::cos(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
+          loc_yaw = -LOC_SIGN_TRIANGLE_ANGLE/2;
+          l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
+          l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
+          l.pose(2) = loc_yaw + world.heading;
+
+          id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2;
+          this->landmarks_[id] = l;
+        }
+        else
+        {
+          if(type == SEMAPHORE_TYPE)
+            l.type = SEMAPHORE_MARKER_ID;
+          else
+            l.type = SIGN_MARKER_ID;
+          world = sign.get_world_pose();
+          l.pose(0) = world.x;
+          l.pose(1) = world.y;
+          l.pose(2) = world.heading;
+          this->landmarks_[(double)sign.get_id()] = l;
+        }
+      }
+    }
+
+  }catch (CException &e){
+    ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgorithm::load_from_xodr -> [Exception caught] : " << e.what());
+    return false;
+  }
+  return true;
+}
+
 bool AdcLandmarksSlamSolverAlgorithm::write_landmarks_file(void)
 {
   std::string file_name;
@@ -188,6 +264,10 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
       double dist = mahalanobis_distance(feature_pos, _feature_r, _feature_th, landmark_pos);
       // std::cout << "check dist " << dist << std::endl;
       //pitch instead of yaw beacuse is a camera
+      while (pitch >= M_PI)
+        pitch -= 2*M_PI;
+      while (pitch < -M_PI)
+        pitch += 2*M_PI;
       if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(pitch -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
       {
         Eigen::Vector2d d = landmark_pos - feature_pos;

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -78,6 +78,12 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
       ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_map_file' not found. Setting it to \"landmarks.txt\".");
       this->config_.landmarks_map_file = "landmarks.txt";
     }
+
+    if(!this->private_node_handle_.getParam("load_landmarks_from_xodr", this->config_.load_landmarks_from_xodr))
+    {
+      ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'load_landmarks_from_xodr' not found. Setting it to false.");
+      this->config_.load_landmarks_from_xodr = false;
+    }
     load_landmarks();
   }
   if (!this->config_.amcl_localization)
@@ -649,6 +655,10 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
     q = tf2::Quaternion(local_feature.pose.orientation.x, local_feature.pose.orientation.y, local_feature.pose.orientation.z, local_feature.pose.orientation.w);
     tf2::Matrix3x3(q).getEulerYPR(yaw_local, pitch, roll);
     // feature_pose_local << local_feature.pose.position.x, local_feature.pose.position.y, yaw_local;
+    while (pitch >= M_PI)
+      pitch -= 2*M_PI;
+    while (pitch < -M_PI)
+      pitch += 2*M_PI;
     feature_pose_local << local_feature.pose.position.z, local_feature.pose.position.x, pitch;//is a camera. axis rotated
 
     (_front? this->front_features_: this->rear_features_)[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose_local, (_front? this->front_features_: this->rear_features_)[i].r, (_front? this->front_features_: this->rear_features_)[i].th, (_front? this->front_features_: this->rear_features_)[i].type, (_front? this->front_features_frame_: this->rear_features_frame_), tf_sensor_map_msg);
@@ -733,6 +743,10 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
 
       double dist = this->alg_.mahalanobis_distance(feature_pos, _feature.r, _feature.th, landmark_pos);
       // pitch instead of yaw beacuse is a camera
+      while (pitch >= M_PI)
+        pitch -= 2*M_PI;
+      while (pitch < -M_PI)
+        pitch += 2*M_PI;
       if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(pitch -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
       {
         Eigen::Vector2d d = landmark_pos - feature_pos;
@@ -887,7 +901,7 @@ bool AdcLandmarksSlamSolverAlgNode::get_tf_map_odom(ros::Time _t, Eigen::Vector3
 void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
 {
   ROS_INFO_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> Loading from file " << this->config_.landmarks_map_file);
-  if (!this->alg_.load_landmarks(this->config_.landmarks_map_file, this->config_.landmarks_pos_fixed))
+  if (!this->alg_.load_landmarks(this->config_.landmarks_map_file, this->config_.landmarks_pos_fixed, this->config_.load_landmarks_from_xodr))
     ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
 }
 
@@ -934,7 +948,7 @@ void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
     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::CUBE;
+    this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::ARROW;
     this->landmarks_MarkerArray_msg_.markers[i].action = visualization_msgs::Marker::ADD;
     // this->landmarks_MarkerArray_msg_.markers[i].lifetime = ros::Duration(0.1);