From 66b561e53d7b6112d5ec2c88fae5d5f02f7f6234 Mon Sep 17 00:00:00 2001
From: Yuta Koga <ykoga.kyutech@gmail.com>
Date: Wed, 27 May 2020 13:54:46 +0900
Subject: [PATCH] Add validation for orientation of initial pose (#317)

* Add validation for orientation of initial pose

* Fix error message

Co-authored-by: Atsushi Watanabe <atsushi.w@ieee.org>
---
 src/mcl_3dl.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp
index 1bfdf135..6b835097 100644
--- a/src/mcl_3dl.cpp
+++ b/src/mcl_3dl.cpp
@@ -166,6 +166,17 @@ class MCL3dlNode
 
   void cbPosition(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
   {
+    const double len2 =
+        msg->pose.pose.orientation.x * msg->pose.pose.orientation.x +
+        msg->pose.pose.orientation.y * msg->pose.pose.orientation.y +
+        msg->pose.pose.orientation.z * msg->pose.pose.orientation.z +
+        msg->pose.pose.orientation.w * msg->pose.pose.orientation.w;
+    if (std::abs(len2 - 1.0) > 0.1)
+    {
+      ROS_ERROR("Discarded invalid initialpose. The orientation must be unit quaternion.");
+      return;
+    }
+
     geometry_msgs::PoseStamped pose_in, pose;
     pose_in.header = msg->header;
     pose_in.pose = msg->pose.pose;