From ddb7df7c508ac429d3511b5359a367bb4136c7ae Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 13 Mar 2026 10:35:53 +0700 Subject: [PATCH] fix bug isQuaternionValid of Goal --- src/APIs/c_api/include/convertor.h | 7 ++++ src/APIs/c_api/src/convertor.cpp | 34 +++++++++++++++++ src/APIs/c_api/src/nav_c_api.cpp | 37 ++++++++++++++++--- .../Packages/move_base/src/move_base.cpp | 10 ++--- 4 files changed, 77 insertions(+), 11 deletions(-) diff --git a/src/APIs/c_api/include/convertor.h b/src/APIs/c_api/include/convertor.h index 6322a2c..30f33af 100644 --- a/src/APIs/c_api/include/convertor.h +++ b/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) diff --git a/src/APIs/c_api/src/convertor.cpp b/src/APIs/c_api/src/convertor.cpp index 4e3e76d..bd7a546 100644 --- a/src/APIs/c_api/src/convertor.cpp +++ b/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; } \ No newline at end of file diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index a6e7d75..fcafddf 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/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 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 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(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(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 nav_ptr = @@ -379,7 +403,6 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const doubl { return false; } - try { std::shared_ptr 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 nav_ptr = diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 1471ce4..0b22f55 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/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; }