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