From ca9d6f7448dfe1014dd35aec80d840dfef039d72 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 7 Mar 2022 15:55:29 +0100
Subject: [PATCH] hotfix: not crashing if no odom available
---
src/processor/processor_odom_icp.cpp | 7 ++++++-
1 file changed, 6 insertions(+), 1 deletion(-)
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 754964104..aa4bbd55e 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -37,6 +37,9 @@ namespace wolf
ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
ProcessorTracker("ProcessorOdomIcp", "PO", 0, _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()),
ro_T_so_(Eigen::Isometry2d::Identity()),
params_odom_icp_(_params)
@@ -107,7 +110,9 @@ void ProcessorOdomIcp::updateExtrinsicsIsometries()
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
if (x1.size() == 3)
--
GitLab