HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix bug isQuaternionValid of Goal

Browse Source
This commit is contained in:
HiepLM
2026-03-13 10:35:53 +07:00
parent 75cbf5a7ef
commit ddb7df7c50
4 changed files with 77 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -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;
}