Add validation for orientation of initial pose (#317)

* Add validation for orientation of initial pose * Fix error message Co-authored-by: Atsushi Watanabe <[email protected]>
at-wat · May 27, 2020 · 66b561e · 66b561e
1 parent e7484d3
commit 66b561e
Showing 1 changed file with 11 additions and 0 deletions.
diff --git 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;