fix bug isQuaternionValid of Goal

src/APIs/c_api/include/convertor.h

@@ -187,6 +187,13 @@ robot_protocol_msgs::Order convert2CppOrder(const Order& order);
  */
 Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);
 
+/**
+ * @brief Check if a quaternion is valid
+ * @param q Quaternion
+ * @return True if valid, false otherwise
+ */
+bool isQuaternionValid(const Quaternion& q);
+
 /**
  * @brief Free dynamic memory held by an Order returned from convert2COrder
  * @param order Order to free (pointers and counts are zeroed)

src/APIs/c_api/src/convertor.cpp

@@ -795,4 +795,38 @@ extern "C" void order_free(Order* order)
     order->edges = nullptr;
   }
   order->edges_count = 0;
+}
+
+bool isQuaternionValid(const Quaternion& q)
+{
+  // first we need to check if the quaternion has nan's or infs
+  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
+  {
+    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
+    return false;
+  }
+
+  tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
+
+  // next, we need to check if the length of the quaternion is close to zero
+  if (tf_q.length2() < 1e-6)
+  {
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
+    return false;
+  }
+
+  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
+  tf_q.normalize();
+
+  tf3::Vector3 up(0, 0, 1);
+
+  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
+
+  if (fabs(dot - 1) > 1e-3)
+  {
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
+    return false;
+  }
+
+  return true;
 }

src/APIs/c_api/src/nav_c_api.cpp

@@ -240,7 +240,11 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped go
   {
     return false;
   }
-
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -264,6 +268,11 @@ extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order or
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -293,6 +302,11 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
 
   try
   {
@@ -301,7 +315,7 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No
             reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
     if (!nav_ptr)
       return false;
-    robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y);
+    robot::log_error("navigation_move_to_nodes_edges goal %f %f", goal.pose.position.x, goal.pose.position.y);
     Order order;
     order.nodes = const_cast<Node *>(nodes);
     order.nodes_count = node_count;
@@ -327,6 +341,11 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
 
   try
   {
@@ -347,10 +366,15 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
 
 extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal) 
 {
-  if (!handle)
+  if (!handle || !marker)
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -379,7 +403,6 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const doubl
   {
     return false;
   }
-
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -411,7 +434,11 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped
   {
     return false;
   }
-
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -2475,7 +2475,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
   // first we need to check if the quaternion has nan's or infs
   if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
   {
-    std::cerr << "Quaternion has nans or infs... discarding as a navigation goal" << std::endl;
+    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
     return false;
   }
 
@@ -2484,7 +2484,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
   // next, we need to check if the length of the quaternion is close to zero
   if (tf_q.length2() < 1e-6)
   {
-    std::cerr << "Quaternion has length close to zero... discarding as navigation goal" << std::endl;
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
     return false;
   }
 
@@ -2497,7 +2497,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
 
   if (fabs(dot - 1) > 1e-3)
   {
-    std::cerr << "Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical." << std::endl;
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
     return false;
   }
 
@@ -2539,9 +2539,7 @@ bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_
   if (!global_pose.header.stamp.isZero() &&
       current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
   {
-    std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". "
-                                                                           "Current time: "
-              << current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl;
+    robot::log_error("Transform timeout for %s. Current time: %f, pose stamp: %f, tolerance: %f", costmap->getName().c_str(), current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
     return false;
   }