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

Fixed no init new_stimated_pose_t

parent 71a43d80
......@@ -109,6 +109,7 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->estimated_sigma_(1) =this->estimated_sigma_(0);
this->estimated_sigma_(2) =this->estimated_sigma_(0);
}
this->new_estimated_pose_t_ = ros::Time::now();
}
else
{
......@@ -234,6 +235,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
if (!this->tf2_buffer.canTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t_, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:mainNodeThread -> Can't transform from " << this->config_.odom_frame << " to " << this->config_.base_link_frame << " at " << this->new_estimated_pose_t_);
// this->new_estimated_pose_ = false;
this->estimated_pose_mutex_exit();
this->features_mutex_exit();
return;
......@@ -299,6 +301,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
frame_data.current_odom = this->tf_odom_base_;
if (!get_tf_map_odom(this->features_stamp_, frame_data.tf_map_odom))
{
// this->new_features_data_ = false;
this->estimated_pose_mutex_exit();
this->features_mutex_exit();
return;
......@@ -310,6 +313,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
{
if (!calculate_robot_state(frame_data.ts, frame_data.robot_state))
{
// this->new_features_data_ = false;
this->estimated_pose_mutex_exit();
this->features_mutex_exit();
return;
......@@ -321,6 +325,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
{
if (!update_landmarks_frame_data(frame_data))
{
// this->new_features_data_ = false;
this->estimated_pose_mutex_exit();
this->features_mutex_exit();
return;
......@@ -491,7 +496,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
if (this->features_frame_ == "")
{
ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::check_landmarks -> Features frame id empty; AR tag filter not initialized.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::check_landmarks -> Features frame id empty; AR tag filter not initialized.");
return false;
}
......
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