From a9600d5cadb478a9ebb41b4b691434e94c338a2c Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iri.upc.edu>
Date: Mon, 4 Oct 2021 11:45:05 +0200
Subject: [PATCH] Fixed bug on traffic sign landmarks orientation

---
 include/adc_landmarks_slam_solver_alg.h    |  2 +-
 src/adc_landmarks_slam_solver_alg.cpp      |  6 +++---
 src/adc_landmarks_slam_solver_alg_node.cpp | 15 +++++++++++----
 3 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/include/adc_landmarks_slam_solver_alg.h b/include/adc_landmarks_slam_solver_alg.h
index ee31f2e..a6eb458 100644
--- a/include/adc_landmarks_slam_solver_alg.h
+++ b/include/adc_landmarks_slam_solver_alg.h
@@ -52,7 +52,7 @@
 #define LOC_SIGN_ID_OFFSET 100
 #define LOC_SIGN_TRIANGLE_WIDTH 0.074
 #define LOC_SIGN_TRIANGLE_ANGLE M_PI/2
-#define LOC_SIGN_TRIANGLE_ANGLE_OFFSET M_PI/2
+#define SIGN_MARKER_ANGLE_OFFSET M_PI/2
 
 //include adc_landmarks_slam_solver_alg main library
 
diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index 53daa4e..71aa4c6 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -119,7 +119,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
           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 + LOC_SIGN_TRIANGLE_ANGLE_OFFSET;
+          l.pose(2) = loc_yaw + world.heading + SIGN_MARKER_ANGLE_OFFSET;
 
           id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2-1;
           this->landmarks_[id] = l;
@@ -129,7 +129,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
           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 + LOC_SIGN_TRIANGLE_ANGLE_OFFSET;
+          l.pose(2) = loc_yaw + world.heading + SIGN_MARKER_ANGLE_OFFSET;
 
           id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2;
           this->landmarks_[id] = l;
@@ -143,7 +143,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
           world = sign.get_world_pose();
           l.pose(0) = world.x;
           l.pose(1) = world.y;
-          l.pose(2) = world.heading;
+          l.pose(2) = world.heading + SIGN_MARKER_ANGLE_OFFSET;
           this->landmarks_[(double)sign.get_id()] = l;
         }
       }
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index f42ba1f..d7159e1 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -1334,11 +1334,18 @@ void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t
   if (config.write_output_files)
   {
     config.write_output_files = false;
-    if (!this->alg_.write_landmarks_file())
+    if (this->must_transform_landmarks_)
     {
-      std::string file_name;
-      file_name = this->config_.output_files_folder + "landmarks.txt";
-      ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode:: Can't open " << file_name << " file.");
+      ROS_WARN("AdcLandmarksSlamSolverAlgNode:: landmarks on opendrive frame. Must be transform to global frame before saving.");
+    }
+    else
+    {
+      if (!this->alg_.write_landmarks_file())
+      {
+        std::string file_name;
+        file_name = this->config_.output_files_folder + "landmarks.txt";
+        ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode:: Can't open " << file_name << " file.");
+      }
     }
   }
   this->config_=config;
-- 
GitLab