diff --git a/include/adc_landmarks_slam_solver_alg.h b/include/adc_landmarks_slam_solver_alg.h index ee31f2ecf0377682029adaf24a4bc98caeca13cd..a6eb4588b1a58c56883b4ed0575b6bfe8c114512 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 53daa4e6c7b08a07114fcab49969d611bcbf7b5b..71aa4c6569d78ce9548bf6c71b454e87a1f42746 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 f42ba1ff949c14fa5756423488f8a3ee032b294e..d7159e12ea6341a5429aa546fa3aefad73745086 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;