Commit a9600d5c authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Fixed bug on traffic sign landmarks orientation

parent 385e66fd
......@@ -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
......
......@@ -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;
}
}
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment