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

fix bug

Browse Source
This commit is contained in:
HiepLM
2026-03-21 19:04:32 +07:00
parent d38f6b3954
commit c05a3e4439
12 changed files with 182 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
View File

@@ -41,6 +41,7 @@
#include <robot_geometry_msgs/Twist.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <robot_nav_msgs/Odometry.h>
#include <memory>
namespace robot_nav_core
@@ -148,6 +149,12 @@ namespace robot_nav_core
*/
virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
/**
* @brief Set the odometry of the robot
* @param odom The odometry of the robot
*/
virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; }
/**
* @brief Virtual destructor for the interface
*/
@@ -155,6 +162,11 @@ namespace robot_nav_core
protected:
BaseLocalPlanner() {}
/**
* @brief The odometry of the robot
*/
robot_nav_msgs::Odometry *odom_;
};
} // namespace robot_nav_core

5
src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
View File

@@ -189,11 +189,6 @@ namespace robot_nav_core_adapter
robot_nav_2d_msgs::Pose2DStamped last_goal_;
bool has_active_goal_;
/**
* @brief The odometry of the robot
*/
robot_nav_msgs::Odometry odom_;
// Plugin handling
boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
robot_nav_core2::LocalPlanner::Ptr planner_;

6
src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -88,7 +88,7 @@ namespace robot_nav_core_adapter
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
std::string planner_name;
if (adapter_nh_.hasParam("planner_name"))
{
@@ -291,7 +291,7 @@ namespace robot_nav_core_adapter
robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
{
return odom_.twist.twist;
return odom_->twist.twist;
}
bool LocalPlannerAdapter::islock()
@@ -356,7 +356,7 @@ namespace robot_nav_core_adapter
if (!getRobotPose(pose2d))
return false;
robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist);
robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist);
bool ret = planner_->isGoalReached(pose2d, velocity);
if (ret)
{

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

@@ -324,6 +324,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
throw std::runtime_error("Failed to load local planner " + local_planner);
}
tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
tc_->setOdom(&odometry_);
}
catch (const std::exception &ex)
{
@@ -1781,32 +1782,6 @@ void move_base::MoveBase::cancel()
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
cancel_ctr_ = true;
robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true");
if (as_)
{
// Get current goal ID from action server to cancel specific goal
// If we want to cancel all goals, use empty string ""
robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
// Use empty string to cancel current goal (processCancel accepts empty string to cancel all)
msg->id = ""; // Empty string cancels current goal
msg->stamp = robot::Time::now();
robot::log_info("[MoveBase::cancel] Sending cancel request to action server");
robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld",
msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec);
// Convert to ConstPtr for processCancel
robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
as_->processCancel(cancel_msg);
robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
}
else
{
robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
}
robot::log_info("[MoveBase::cancel] ===== EXIT =====");
}
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
@@ -2381,8 +2356,9 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
}
}
robot::log_warning("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y);
publishZeroVelocity();
// we have a goal so start the planner
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2602,7 +2578,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
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
@@ -2764,6 +2739,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
recovery_index_ = 0;
}
// Cancle executeCycle
if (cancel_ctr_ && tc_)
{
@@ -2772,13 +2748,25 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
tc_->setTwistLinear(linear);
try
{
if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
if (actual_linear_velocity <= min_approach_linear_velocity_)
{
robot::log_info("MoveTo is canceled.");
resetState();
cancel_ctr_ = false;
// if(default_config_.base_global_planner != last_config_.base_global_planner)
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
cancel_ctr_ = false;
// disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
@@ -2789,7 +2777,21 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
}
// as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
cancel_ctr_ = false;
if (as_)
{
robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
msg->id = ""; // Empty string cancels current goal
msg->stamp = robot::Time::now();
// Convert to ConstPtr for processCancel
robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
as_->processCancel(cancel_msg);
robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
}
else
{
robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
}
return true;
}
}
@@ -2798,7 +2800,17 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
robot::log_info("MoveTo is canceled.");
resetState();
// if(default_config_.base_global_planner != last_config_.base_global_planner)
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
// disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2810,6 +2822,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
}
// as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
if (as_)
{
robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
msg->id = ""; // Empty string cancels current goal
msg->stamp = robot::Time::now();
// Convert to ConstPtr for processCancel
robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
as_->processCancel(cancel_msg);
robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
}
else
{
robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
}
cancel_ctr_ = false;
return true;
}
@@ -2830,7 +2856,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
// if we're controlling, we'll attempt to find valid velocity commands
case move_base::CONTROLLING:
// robot::log_debug("In controlling state.");
// check to see if we've reached our goal
try
@@ -3004,7 +3029,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
tc_->setTwistLinear(linear);
try
{
if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
if (actual_linear_velocity <= min_approach_linear_velocity_)
{
paused_ = true;
}
@@ -3108,15 +3134,48 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
{
std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
robot_geometry_msgs::PoseStamped goal_pose, global_pose;
goal_pose = goal_pose_msg;
goal_pose.header.stamp = robot::Time(); // latest available
if (!tf_)
{
return goal_pose_msg;
}
std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
// robot_geometry_msgs::PoseStamped goal_pose, global_pose;
// goal_pose = goal_pose_msg;
// goal_pose.header.stamp = robot::Time(); // latest available
// try
// {
// tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
// tf3::doTransform(goal_pose, global_pose, transform);
// }
robot_geometry_msgs::PoseStamped global_pose;
tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
robot_geometry_msgs::PoseStamped goal_pose;
tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose);
goal_pose = goal_pose_msg;
robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
tf3::Time tf3_current_time = data_convert::convertTime(current_time);
tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
try
{
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
tf3::doTransform(goal_pose, global_pose, transform);
// use current time if possible (makes sure it's not in the future)
std::string error_msg;
if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg))
{
// Transform is available at current time
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time);
tf3::doTransform(goal_pose, global_pose, transform);
}
// use the latest otherwise (tf3::Time() means latest available)
else
{
// Try to get latest transform
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time);
tf3::doTransform(goal_pose, global_pose, transform);
}
}
catch (tf3::LookupException &ex)
{