Commit 5e1adb03 authored by asantamaria's avatar asantamaria

minor change

parent d485d078
......@@ -308,20 +308,20 @@ void EskfOdomAlgNode::flow2d_callback(const std_msgs::Float32MultiArray::ConstPt
else
ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Optical Flow Message Received. Possible Causes: Stamp, Label or Data Size.");
//OPTIONAL: COVARIANCE from the message.
if (msg->layout.dim[2].label=="covariance" && msg->layout.dim[2].size == 4)
{
this->alg_.lock();
Sensor::flow2d_params flow2d_params = this->alg_.get_flow2d_params();
if (flow2d_params.std == flow2d_params.std_insidebounds)
{
flow2d_params.std << msg->data[3],msg->data[6];
this->alg_.set_flow2d_params(flow2d_params);
}
this->alg_.unlock();
}
else
ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Covariance Message Received. Possible Causes: Label or Data Size.");
// //OPTIONAL: COVARIANCE from the message.
// if (msg->layout.dim[2].label=="covariance" && msg->layout.dim[2].size == 4)
// {
// this->alg_.lock();
// Sensor::flow2d_params flow2d_params = this->alg_.get_flow2d_params();
// if (flow2d_params.std == flow2d_params.std_insidebounds)
// {
// flow2d_params.std << msg->data[3],msg->data[6];
// this->alg_.set_flow2d_params(flow2d_params);
// }
// this->alg_.unlock();
// }
// else
// ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Covariance Message Received. Possible Causes: Label or Data Size.");
}
else
ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Message Layout Received");
......
Markdown is supported
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