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

update bug crash miss docking

Browse Source
This commit is contained in:
hoangson02
2026-04-24 06:48:26 +00:00
parent 08d597304e
commit ece9154a84
6 changed files with 221 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@@ -117,7 +117,9 @@ namespace mkt_algorithm
* @return lookahead point
*/
std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity,
const double &lookahead_dist,
robot_nav_2d_msgs::Path2D& global_plan);
/**
* @brief Prune global plan

131
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -279,6 +279,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
double &x_direction, double &y_direction, double &theta_direction)
{
// robot::log_error("DEBUG STEP 2.0");
if (!initialized_)
{
robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
@@ -288,7 +289,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
{
last_actuator_update_ = robot::Time::now();
}
// robot::log_error("DEBUG STEP 3.0");
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
if (footprint.size() > 1)
{
@@ -311,14 +312,14 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
}
// robot::log_error("DEBUG STEP 4.0");
if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
{
robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
return false;
}
this->getParams();
// robot::log_error("DEBUG STEP 5.0");
frame_id_path_ = global_plan.header.frame_id;
goal_ = goal;
global_plan_ = global_plan;
@@ -329,14 +330,14 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__);
return false;
}
// robot::log_error("DEBUG STEP 6.0");
double S = std::numeric_limits<double>::infinity();
S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0,
costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0);
const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_;
S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S);
compute_plan_.poses.clear();
// robot::log_error("DEBUG STEP 7.0");
if ((unsigned int)global_plan_.poses.size() == 2)
{
double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x;
@@ -360,6 +361,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
return false;
}
}
// robot::log_error("DEBUG STEP 8.0");
double lookahead_dist = this->getLookAheadDistance(velocity);
transform_plan_.poses.clear();
@@ -368,6 +370,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false;
}
// robot::log_error("DEBUG STEP 9.0");
const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
if(fabs(carrot_pose.pose.y) > 0.2)
{
@@ -378,14 +381,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
return false;
}
}
// robot::log_error("DEBUG STEP 10.0");
x_direction = x_direction_;
y_direction = y_direction_ = 0;
theta_direction = theta_direction_;
// robot::log_error("DEBUG STATUS : %x, %x", (unsigned int)(compute_plan_.poses.size() > 1), journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution());
if ((unsigned int)(compute_plan_.poses.size() > 1) &&
journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution())
{
// robot::log_error("DEBUG STEP 10.1");
const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
int index;
for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--)
@@ -406,12 +410,19 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
}
else
{
// robot::log_error("DEBUG STEP 11.1");
try
{
auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
// robot::log_error("DEBUG STEP 11.2");
auto prev_carrot_pose_it = transform_plan_.poses.begin();
// robot::log_error("DEBUG STEP 11.2.1 carrot_pose_it: %d", (int)std::distance(transform_plan_.poses.begin(), carrot_pose_it));
double distance_it = 0;
for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
auto it = carrot_pose_it == transform_plan_.poses.begin() ? transform_plan_.poses.end() : carrot_pose_it - 1;
for ( ; it != transform_plan_.poses.begin(); --it)
{
double dx = it->pose.x - carrot_pose_it->pose.x;
double dy = it->pose.y - carrot_pose_it->pose.y;
@@ -422,19 +433,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
break;
}
}
// robot::log_error("DEBUG STEP 11.3");
robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
: robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
// robot::log_error("DEBUG STEP 11.4");
// teb_local_planner::PoseSE2 start_pose(front);
// teb_local_planner::PoseSE2 goal_pose(back);
// const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
const double dir_path = 0.0;
if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
x_direction = dir_path > 0 ? FORWARD : BACKWARD;
// robot::log_error("DEBUG STEP 11.5");
}
catch (std::exception &e)
{
@@ -442,7 +455,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
return false;
}
}
// robot::log_error("DEBUG STEP 11.0");
x_direction_ = x_direction;
y_direction_ = y_direction;
theta_direction_ = theta_direction;
@@ -1073,7 +1086,8 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
}
std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity,
const double &lookahead_dist, robot_nav_2d_msgs::Path2D& global_plan)
{
if (global_plan.poses.empty())
throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
@@ -1110,9 +1124,104 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
if (goal_pose_it == global_plan.poses.end())
goal_pose_it = std::prev(global_plan.poses.end());
// --- Final safety check ---
if (goal_pose_it < global_plan.poses.begin() || goal_pose_it >= global_plan.poses.end())
{
// fallback cuối cùng
goal_pose_it = std::prev(global_plan.poses.end());
}
return goal_pose_it;
}
// std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
// mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(
// const robot_nav_2d_msgs::Twist2D &velocity,
// const double &lookahead_dist,
// robot_nav_2d_msgs::Path2D &global_plan)
// {
// auto &poses = global_plan.poses;
// // --- Guard ---
// if (poses.empty())
// throw robot_nav_core2::PlannerTFException("The global plan is empty.");
// if (poses.size() == 1)
// return poses.begin();
// if (poses.size() == 2)
// return std::prev(poses.end());
// // --- Init ---
// size_t goal_index = poses.size() - 1;
// const auto &p0 = poses[0].pose;
// const auto &p1 = poses[1].pose;
// double start_angle = atan2(p1.y - p0.y, p1.x - p0.x);
// double turn_threshold = M_PI_2 * 0.6;
// // --- Detect turn ---
// for (size_t i = 1; i < poses.size(); ++i)
// {
// const auto &a = poses[i - 1].pose;
// const auto &b = poses[i].pose;
// double current_angle = atan2(b.y - a.y, b.x - a.x);
// double delta = angles::normalize_angle(current_angle - start_angle);
// goal_index = i;
// if (fabs(delta) >= turn_threshold)
// break;
// }
// // --- Clamp goal_index ---
// if (goal_index >= poses.size())
// goal_index = poses.size() - 1;
// // --- Safe search range ---
// auto search_begin = poses.begin();
// // ❗ IMPORTANT: +1 để iterator hợp lệ
// auto search_end = poses.begin() + goal_index + 1;
// if (search_end > poses.end())
// search_end = poses.end();
// // --- Find lookahead ---
// double accumulated_dist = 0.0;
// auto goal_pose_it = search_begin;
// for (auto it = search_begin + 1; it != search_end; ++it)
// {
// double dx = it->pose.x - std::prev(it)->pose.x;
// double dy = it->pose.y - std::prev(it)->pose.y;
// accumulated_dist += std::hypot(dx, dy);
// if (accumulated_dist >= lookahead_dist)
// {
// goal_pose_it = it;
// break;
// }
// }
// // --- Fallback an toàn ---
// if (goal_pose_it == search_begin)
// {
// goal_pose_it = std::prev(search_end); // safe vì search_end > search_begin
// }
// // --- Final safety check ---
// if (goal_pose_it < poses.begin() || goal_pose_it >= poses.end())
// {
// // fallback cuối cùng
// return std::prev(poses.end());
// }
// return goal_pose_it;
// }
bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot)
{
if (global_plan.poses.empty())

2
src/Algorithms/Packages/global_planners/custom_planner

Submodule src/Algorithms/Packages/global_planners/custom_planner updated: 7c84705ce7...62a493a892

2
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h
View File

@@ -31,6 +31,8 @@ namespace pnkx_local_planner
void initialize(robot::NodeHandle &parent, const std::string &name,
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
/**
* @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*

53
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@@ -240,6 +240,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getParams(robot::NodeHandle &n
}
}
void pnkx_local_planner::PNKXDockingLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path)
{
this->reset();
pnkx_local_planner::PNKXLocalPlanner::setPlan(path);
}
void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
{
robot::log_info_at(__FILE__, __LINE__, "New Docking Goal Received.");
@@ -260,6 +266,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.0");
this->getParams(planner_nh_);
if (update_costmap_before_planning_)
{
@@ -273,11 +280,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
if (!costmap_robot_->isCurrent())
throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
}
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 2.0");
// Update time stamp of goal pose
// goal_pose_.header.stamp = pose.header.stamp;
robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
local_goal_pose = this->transformPoseToLocal(goal_pose_);
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 3.0");
if (start_docking_)
{
local_goal_pose = goal_pose_;
@@ -285,6 +293,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
try
{
// robot::log_error("local_start_pose (%f, %f, %f)", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
// robot::log_error("local_goal_pose (%f, %f, %f)", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
// for(size_t i = 0; i < global_plan_.poses.size(); i++)
// {
// robot::log_error("global_plan_ [%zu] (%f, %f, %f)", i, global_plan_.poses[i].pose.x, global_plan_.poses[i].pose.y, global_plan_.poses[i].pose.theta);
// }
// robot::log_error("costmap_robot_->getGlobalFrameID(): %s", costmap_robot_->getGlobalFrameID().c_str());
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
}
@@ -294,7 +310,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
}
double x_direction, y_direction, theta_direction;
if (!ret_nav_)
if (!ret_nav_ && !dkpl_.empty())
{
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 4.0");
if(nav_algorithm_)
{
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
@@ -303,9 +322,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
}
// else
// ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 5.0");
if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
{
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 6.0");
this->lock();
robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
traj_generator_->setTwistLinear(linear);
@@ -324,7 +344,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 7.0");
if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
{
@@ -342,6 +362,9 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
}
}
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 8.0");
}
}
}
else
@@ -365,14 +388,26 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
const robot_nav_2d_msgs::Twist2D &velocity)
{
// boost::recursive_mutex::scoped_lock l(configuration_mutex_);
// // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
// for(size_t i = 0; i < global_plan_.poses.size(); i++)
// {
// robot::log_error("computeVelocityCommands global_plan_ [%d] (%f, %f, %f)", i, global_plan_.poses[i].pose.x, global_plan_.poses[i].pose.y, global_plan_.poses[i].pose.theta);
// }
robot_nav_2d_msgs::Twist2DStamped cmd_vel;
cmd_vel.header.stamp = robot::Time::now();
cmd_vel.velocity.x = 0.0;
cmd_vel.velocity.y = 0.0;
cmd_vel.velocity.theta = 0.0;
try
{
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.0");
if ( global_plan_.poses.empty())
return cmd_vel;
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.11");
this->prepare(pose, velocity);
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 2");
cmd_vel = this->ScoreAlgorithm(pose, velocity);
// robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 3");
return cmd_vel;
}
catch (const robot_nav_core2::PlannerException &e)
@@ -419,6 +454,11 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
// if(global_plan_.poses.size() <= 2)
// {
// robot::log_error("DEBUG GOAL");
// return true;
// }
if (goal_pose_.header.frame_id == "")
{
robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
@@ -511,6 +551,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
dkpl_.front()->getLocalPath(local_pose, local_goal, path);
this->setPlan(robot_nav_2d_utils::pathToPath(path));
this->setGoalPose(local_goal);
robot::log_debug(__FILE__, __LINE__, "DEBUG 1 size path: %d", (int)path.poses.size());
}
}
catch (const std::exception &e)
@@ -535,6 +576,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
path.poses.push_back(local_goal);
this->setPlan(path);
this->setGoalPose(local_goal);
robot::log_debug(__FILE__, __LINE__, "DEBUG 2 size path: %d", (int)path.poses.size());
}
}
catch (const std::exception &e)
@@ -543,6 +585,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
}
}
}
}
else
{

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

@@ -1278,6 +1278,7 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
}
robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
}
catch (const std::exception &e)
@@ -2494,6 +2495,7 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
// the real work on pursuing a goal is done here
bool done = executeCycle(goal);
// robot::log_debug("[MoveBase] Completed an execution cycle: ̀done=%s", done ? "true" : "false");
// if we're done, then we'll return from execute
if (done)
return;
@@ -2713,7 +2715,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
controller_plan_ = latest_plan_;
latest_plan_ = temp_plan;
lock.unlock();
robot::log_debug("pointers swapped!");
robot::log_debug("pointers swapped!: %d", controller_plan_->size());
if (!tc_->setPlan(*controller_plan_))
{
@@ -2734,6 +2736,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
// as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
return true;
}
// robot::log_debug("pointers swapped2!");
// make sure to reset recovery_index_ since we were able to find a valid plan
if (recovery_trigger_ == PLANNING_R)
@@ -2745,6 +2748,9 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
if (cancel_ctr_ && tc_)
{
robot_geometry_msgs::Vector3 linear;
linear.x = 0.0;
linear.y = 0.0;
linear.z = 0.0;
// ROS_INFO_THROTTLE(1.0,"MoveTo is Canling ....");
tc_->setTwistLinear(linear);
try
@@ -2914,10 +2920,12 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
// robot::log_error("paused_: %s", paused_ ? "true" : "false");
if (!paused_)
{
if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
{
// robot::log_debug("Got a valid velocity command from the local planner start!");
robot_nav_msgs::Path path;
tc_->getPlan(path.poses);
if (!path.poses.empty())
@@ -2943,6 +2951,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
}
if (recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
// robot::log_debug("Got a valid velocity command from the local planner end!");
}
else
{