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

uodate

Browse Source
This commit is contained in:
HiepLM
2026-01-14 17:53:27 +07:00
parent 6549425279
commit 0f58db3481
11 changed files with 125 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -188,7 +188,7 @@ namespace robot_nav_core_adapter
}
catch (const robot_nav_core2::PlannerException &e)
{
std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
robot::log_error_at(__FILE__, __LINE__, "LocalPlannerAdapter computeVelocityCommands exception: %s", e.what());
return false;
}
cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);

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

@@ -1246,6 +1246,10 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
robot::log_error("Failed to find a plan without Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
return false;
}
else
{
global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
}
lock.unlock();
return true;
}
@@ -1280,6 +1284,10 @@ bool move_base::MoveBase::makePlan(const robot_protocol_msgs::Order &msg, const
robot::log_error("Failed to find a plan with Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
return false;
}
else
{
global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
}
lock.unlock();
return true;
}
@@ -2136,7 +2144,7 @@ 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.");
// robot::log_debug("In controlling state.");
// check to see if we've reached our goal
try
@@ -2198,12 +2206,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{
if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
{
robot::log_debug("Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
last_valid_control_ = robot::Time::now();
// make sure that we send the velocity command to the base
// vel_pub_.publish(cmd_vel);
if (nav_feedback_)
{
@@ -2216,8 +2220,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
}
else
{
robot::log_debug("The local planner could not find a valid plan.");
robot::log_warning("The local planner could not find a valid plan.");
robot::log_warning_at(__FILE__, __LINE__, "The local planner could not find a valid plan.");
robot::Time attempt_end = last_valid_control_ + robot::Duration(controller_patience_);
// check if we've tried to find a valid control for longer than our time limit
@@ -2246,34 +2249,34 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
// enable the planner thread in case it isn't running on a clock
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
// if (as_->isPreemptRequested())
// {
// if (as_->isNewGoalAvailable())
// {
// // if we're active and a new goal is available, we'll accept it, but we won't shut anything down
// move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
// if (!isQuaternionValid(new_goal.target_pose.pose.orientation))
// {
// if (nav_feedback_)
// {
// nav_feedback_->navigation_state = move_base_core::State::ABORTED;
// nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion");
// }
// as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
// return false;
// }
// else
// {
// if (nav_feedback_)
// {
// nav_feedback_->navigation_state = move_base_core::State::ACTIVE;
// nav_feedback_->feed_back_str = std::string("The new goal is received");
// }
// }
// goal = goalToGlobalFrame(new_goal.target_pose);
// planner_goal_ = goal;
// }
// }
if (as_->isPreemptRequested())
{
if (as_->isNewGoalAvailable())
{
// if we're active and a new goal is available, we'll accept it, but we won't shut anything down
robot_move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
if (!isQuaternionValid(new_goal.target_pose.pose.orientation))
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion");
}
as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return false;
}
else
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE;
nav_feedback_->feed_back_str = std::string("The new goal is received");
}
}
goal = goalToGlobalFrame(new_goal.target_pose);
planner_goal_ = goal;
}
}
runPlanner_ = true;
cancel_ctr_ = false;
planner_cond_.notify_one();
@@ -2311,21 +2314,13 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
}
break;
// we'll try to clear out space with any user-provided recovery behaviors
// we'll try to clear out space with any user-provided recovery behaviors
case move_base::CLEARING:
robot::log_debug("[%s:%d] In clearing/recovery state", __FILE__, __LINE__);
// we'll invoke whatever recovery behavior we're currently on if they're enabled
if (recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())
{
robot::log_debug("[%s:%d] Executing behavior %u of %zu", __FILE__, __LINE__, recovery_index_ + 1, recovery_behaviors_.size());
// move_base_msgs::RecoveryStatus msg;
// msg.pose_stamped = current_position;
// msg.current_recovery_number = recovery_index_;
// msg.total_number_of_recoveries = recovery_behaviors_.size();
// msg.recovery_behavior_name = recovery_behavior_names_[recovery_index_];
// recovery_status_pub_.publish(msg);
recovery_behaviors_[recovery_index_]->runBehavior();
// we at least want to give the robot some time to stop oscillating after executing the behavior
@@ -2363,7 +2358,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
nav_feedback_->feed_back_str = std::string("Failed to find a valid control. Even after executing recovery behaviors.");
}
// as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
}
else if (recovery_trigger_ == PLANNING_R)
{
@@ -2373,7 +2368,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
nav_feedback_->feed_back_str = "Failed to find a valid plan. Even after executing recovery behaviors.";
}
// as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
}
else if (recovery_trigger_ == OSCILLATION_R)
{
@@ -2383,7 +2378,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
nav_feedback_->feed_back_str = std::string("Robot is oscillating. Even after executing recovery behaviors.");
}
// as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
}
return true;
}
@@ -2400,7 +2395,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
nav_feedback_->feed_back_str = std::string("Reached a case that should not be hit in move_base. This is a bug, please report it.");
}
// as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
return true;
}