Skip to content
Snippets Groups Projects
Commit ca9d6f74 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

hotfix: not crashing if no odom available

parent 178f56d2
No related branches found
No related tags found
1 merge request!32new tag
...@@ -37,6 +37,9 @@ namespace wolf ...@@ -37,6 +37,9 @@ namespace wolf
ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params): ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
ProcessorTracker("ProcessorOdomIcp", "PO", 0, _params), ProcessorTracker("ProcessorOdomIcp", "PO", 0, _params),
MotionProvider("PO", _params), MotionProvider("PO", _params),
odom_origin_(Eigen::VectorXd::Zero(3)),
odom_last_(Eigen::VectorXd::Zero(3)),
odom_incoming_(Eigen::VectorXd::Zero(3)),
rl_T_sl_(Eigen::Isometry2d::Identity()), rl_T_sl_(Eigen::Isometry2d::Identity()),
ro_T_so_(Eigen::Isometry2d::Identity()), ro_T_so_(Eigen::Isometry2d::Identity()),
params_odom_icp_(_params) params_odom_icp_(_params)
...@@ -107,7 +110,9 @@ void ProcessorOdomIcp::updateExtrinsicsIsometries() ...@@ -107,7 +110,9 @@ void ProcessorOdomIcp::updateExtrinsicsIsometries()
Eigen::Isometry2d ProcessorOdomIcp::computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2 ) const Eigen::Isometry2d ProcessorOdomIcp::computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2 ) const
{ {
assert((x1.size() == 3 and x2.size() == 3) or (x1.size() == 7 and x2.size() == 7)); // any vector not properly filled
if (not (x1.size() == 3 and x2.size() == 3) and not (x1.size() == 7 and x2.size() == 7))
return Eigen::Isometry2d::Identity();
// 2D // 2D
if (x1.size() == 3) if (x1.size() == 3)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment