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

Fixed bug accessing always front_features_stamp

parent 622c6ab4
......@@ -708,7 +708,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
landmark.source_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
landmark.type = (_front? this->front_features_: this->rear_features_)[i].type;
landmark.fixed = false;
double landmark_key = std::fmod(this->front_features_stamp_.toSec(), L_KEY_DIV)*100+i;
double landmark_key = std::fmod(this->features_stamp_.toSec(), L_KEY_DIV)*100+i;
(_front? this->front_features_: this->rear_features_)[i].landmark_key = landmark_key;
this->alg_.add_landmark(landmark_key, landmark);
ROS_DEBUG_STREAM("New landmark " << landmark_key << " at " << (_front? this->front_features_: this->rear_features_)[i].pose(0) << ", " << (_front? this->front_features_: this->rear_features_)[i].pose(1) << "; yaw " << (_front? this->front_features_: this->rear_features_)[i].pose(2));
......
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