Added tranformation from opendrive to map

README.md

 ## Description
 
-This node provides a global localization based on landmarks. On the ADC competition, these landmarks are [ar_track_alvar](http://wiki.ros.org/ar_track_alvar) tags. This node takes the filtered ar tag detections of both cameras and optimizes a landmark based slam problem to get the robot position. A landmarks based slam problem is the problem of optimize a window of robot poses with a map of landmarks. It uses [Ceres Solver](http://ceres-solver.org/index.html) to make the optimization.
+This node provides a global localization based on landmarks. On the ADC competition, these landmarks are [ar_track_alvar](http://wiki.ros.org/ar_track_alvar) tags. This node takes the filtered ar tag detections of both cameras and optimizes a landmark based slam problem to get the robot position. A landmarks based slam problem is the problem of optimize a number of robot poses with a map of landmarks. It uses [Ceres Solver](http://ceres-solver.org/index.html) to make the optimization.
 
 The following image is a skecth of the input and output of the node.
 
@@ -28,13 +28,13 @@ On [parameters_adjust.md](./doc/parameters_adjust.md) there is an small explanat
   - /**tf** (tf/tfMessage): Tf to listen the transforms between odom an robot's base link and between the robot's base link and the sensors.
   - ~**front_features** (iri_adc_msgs/feature_array.msg): Incoming detections from the front camera.
   - ~**rear_features** (iri_adc_msgs/feature_array.msg): Incoming detections from the rear camera.
-
 ### Parameters
 #####General
 - ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz. 
 - ~**global_frame** (String; default: map) Global frame id. 
 - ~**odom_frame** (String; default: odom) Odometry frame id. 
 - ~**base_link_frame** (String; default: base_link) Robot's base link frame id. 
+- ~**opendriveframe** (String; default: opendrive) Opendrive frame id. 
 - ~**tf_timeout** (Double; default: 0.2; min: 0.1; max: 2.0) Timeout to find a transform. 
 - ~**old_feature_timeout** (Double; default: 0.5; min: 0.1; max: 2.0) Timeout to set a features source as old. 
 - ~**amcl_pose_estimated_sigma** (Double; default: 1.0; min: 0.1; max: 10.0) AMCL pose sigma when using AMCL localization.
@@ -59,6 +59,8 @@ On [parameters_adjust.md](./doc/parameters_adjust.md) there is an small explanat
 - ~**update_problem_features_detected** (Integer; default: 3; min: 1; max: 10) Update problem when a lot of features are detected.
 - ~**wait_feature_detected_timeout** (Double; default: 0.2; min: 0.01; max: 0.5) Timeout on a update problem event to wait for a feature detection.
 - ~**landmarks_map_file** (String; default: landmarks.txt) Landmarks map txt file path. 
+- ~**load_landmarks_from_xodr** (Boolean; default: False) Boolean to load landmarks from a xodr file.
+- ~**landmarks_xodr_scale** (Double; default: 1.0; min: 0.0) Landmarks xodr scale factor.
 - ~**problem_frame_window** (Integer; default: 120; min: 50; max: 1000) Max number of frames to add.
 ##### Flags
 - ~**publish_visualization** (Boolean; default: False) Boolean to publish visualization markers.
@@ -112,6 +114,7 @@ This node provides a basic launch file named **node.launch** intended to be incl
 - *launch_prefix*: Node's launch prefix.
 - *config_file*: Path to the configuration file.
 - *landmarks_map_file*: Landmarks txt map file.
+- *load_landmarks_from_xodr*: Flag to know if landmars are loaded from a xodr file.
 - *initial_pose_x*: Initial robot x position.
 - *initial_pose_y*: Initial robot y position.
 - *initial_pose_yaw*: Initial robot orientation.

cfg/AdcLandmarksSlamSolver.cfg

@@ -47,6 +47,7 @@ gen.add("rate",                               double_t,     0,           "Main l
 gen.add("global_frame",                       str_t,        0,           "Global frame",                                                              "map")
 gen.add("odom_frame",                         str_t,        0,           "Odom frame",                                                                "odom")
 gen.add("base_link_frame",                    str_t,        0,           "Robot base_link frame",                                                     "base_link")
+gen.add("opendrive_frame",                    str_t,        0,           "Opendrive frame",                                                           "opendrive")
 gen.add("tf_timeout",                         double_t,     0,           "Timeout to find a transform",                                               0.2, 0.1, 2.0)
 gen.add("old_feature_timeout",                double_t,     0,           "Timeout to set a features source as old",                                   0.5, 0.1, 2.0)
 gen.add("amcl_pose_estimated_sigma",          double_t,     0,           "AMCL pose sigma when using AMCL localization",                              1, 0.1, 10)

include/adc_landmarks_slam_solver_alg.h

@@ -161,6 +161,13 @@ class AdcLandmarksSlamSolverAlgorithm
      */
     void add_landmark(double _landmark_key, const LandmarkInfo& _landmark);
 
+    /**
+     * \brief Function to apply a transform to landmarks.
+     *
+     * \param _tf_map_opendrive The transform to be applied.
+     */
+    void transform_landmarks(geometry_msgs::TransformStamped& _tf_map_opendrive);
+
     /**
      * \brief Function to delete all landmarks.
      */

include/adc_landmarks_slam_solver_alg_node.h

@@ -123,6 +123,8 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
 
     bool last_frame_min_detections_; ///< Flag to know that last frame was because a lot of front_features seen.
 
+    bool must_transform_landmarks_;///< Flag to know if landmarks must be transformed from opendrive frame to global frame.
+
     // [publisher attributes]
     ros::Publisher frame_data_publisher_;
     visualization_msgs::MarkerArray frame_data_MarkerArray_msg_;

src/adc_landmarks_slam_solver_alg.cpp

@@ -156,6 +156,32 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
   return true;
 }
 
+void AdcLandmarksSlamSolverAlgorithm::transform_landmarks(geometry_msgs::TransformStamped& _tf_map_opendrive)
+{
+  for (std::map<double, LandmarkInfo>::iterator it = this->landmarks_.begin(); it != this->landmarks_.end(); ++it)
+  {
+    geometry_msgs::PoseStamped land, transformed_land;
+    land.header.stamp = _tf_map_opendrive.header.stamp;
+    land.header.frame_id = _tf_map_opendrive.child_frame_id;//global_frame
+    land.pose.position.z = 0.0;
+    land.pose.position.x = it->second.pose(0);
+    land.pose.position.y = it->second.pose(1);
+    tf2::Quaternion q;
+    q.setRPY(0.0, 0.0, it->second.pose(2));
+    land.pose.orientation = tf2::toMsg(q);
+
+    tf2::doTransform(land, transformed_land, _tf_map_opendrive);
+
+    double yaw, pitch, roll;
+    q = tf2::Quaternion(transformed_land.pose.orientation.x, transformed_land.pose.orientation.y, transformed_land.pose.orientation.z, transformed_land.pose.orientation.w);
+    tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+
+    it->second.pose(0) = transformed_land.pose.position.x;
+    it->second.pose(1) = transformed_land.pose.position.y;
+    it->second.pose(2) = yaw;
+  }
+}
+
 bool AdcLandmarksSlamSolverAlgorithm::write_landmarks_file(void)
 {
   std::string file_name;

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -89,8 +89,13 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
     {
       if(!this->private_node_handle_.getParam("landmarks_xodr_scale", this->config_.landmarks_xodr_scale))
       {
-        ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_xodr_scale' not found. Setting it to false.");
-        this->config_.landmarks_xodr_scale = false;
+        ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_xodr_scale' not found. Setting it to 1.0.");
+        this->config_.landmarks_xodr_scale = 1.0;
+      }
+      if(!this->private_node_handle_.getParam("opendrive_frame", this->config_.opendrive_frame))
+      {
+        ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'opendrive_frame' not found. Setting it to \"opendrive\".");
+        this->config_.opendrive_frame = "opendrive";
       }
     }
     load_landmarks();
@@ -571,6 +576,22 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
   geometry_msgs::TransformStamped tf_map_sensor_msg, tf_sensor_map_msg;
   geometry_msgs::PoseStamped local_feature, global_feature;
 
+  if (this->must_transform_landmarks_)
+  {
+    geometry_msgs::TransformStamped tf_map_opendrive_msg;
+    tf_map_opendrive_msg.header.frame_id = this->config_.global_frame;
+    tf_map_opendrive_msg.header.stamp = ros::Time::now();
+    tf_map_opendrive_msg.child_frame_id = this->config_.opendrive_frame;
+    if (!this->tf2_buffer.canTransform(tf_map_opendrive_msg.header.frame_id, tf_map_opendrive_msg.child_frame_id, tf_map_opendrive_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
+    {
+      ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << tf_map_opendrive_msg.header.frame_id << " to " << tf_map_opendrive_msg.child_frame_id << " at " << tf_map_opendrive_msg.header.stamp);
+      return false;
+    }
+    tf_map_opendrive_msg = this->tf2_buffer.lookupTransform(tf_map_opendrive_msg.header.frame_id, tf_map_opendrive_msg.child_frame_id, tf_map_opendrive_msg.header.stamp);
+    this->alg_.transform_landmarks(tf_map_opendrive_msg);
+    this->must_transform_landmarks_ = false;
+  }
+
   if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
   {
     ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
@@ -912,6 +933,10 @@ 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, this->config_.load_landmarks_from_xodr, this->config_.landmarks_xodr_scale))
     ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
+  if (this->config_.load_landmarks_from_xodr)
+    this->must_transform_landmarks_ = true;
+  else
+    this->must_transform_landmarks_ = false;
 }
 
 void AdcLandmarksSlamSolverAlgNode::merge_features()