HiepLM/pnkx_nav_core
1
0
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
View File

@ -201,7 +201,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
{ {
sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front(); sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl; // std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl;
} }
// Process index_s with multiple elements // Process index_s with multiple elements

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

@ -790,7 +790,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
robot_nav_2d_msgs::Pose2DStamped robot; robot_nav_2d_msgs::Pose2DStamped robot;
if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot))
{ {
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
} }
double dist_thresh_sq = dist_behind_robot * dist_behind_robot; double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
@ -855,7 +855,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
robot_nav_2d_msgs::Pose2DStamped robot_pose; robot_nav_2d_msgs::Pose2DStamped robot_pose;
if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
{ {
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
} }
// we'll discard points on the plan that are outside the local costmap // we'll discard points on the plan that are outside the local costmap

12
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@ -226,15 +226,16 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
// Update time stamp of goal pose // Update time stamp of goal pose
// goal_pose_.header.stamp = pose.header.stamp; // goal_pose_.header.stamp = pose.header.stamp;
// robot::log_info("pose: %f %f %f", pose.pose.x, pose.pose.y, pose.pose.theta);
robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
local_goal_pose = this->transformPoseToLocal(goal_pose_); local_goal_pose = this->transformPoseToLocal(goal_pose_);
// robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
if (pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) // robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
{ {
robot::log_debug_at(__FILE__, __LINE__, "Transform global plan is successful");
}
else
robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed"); robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
}
double x_direction, y_direction, theta_direction; double x_direction, y_direction, theta_direction;
if (!ret_nav_) if (!ret_nav_)
@ -276,6 +277,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeV
} }
catch (const robot_nav_core2::PlannerException &e) catch (const robot_nav_core2::PlannerException &e)
{ {
robot::log_warning_at(__FILE__, __LINE__, "PNKXLocalPlanner computing velocity commands has been broken: %s", e.what());
throw robot_nav_core2::LocalPlannerException("computing velocity commands has been broken"); throw robot_nav_core2::LocalPlannerException("computing velocity commands has been broken");
return cmd_vel; return cmd_vel;
} }

6
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp
View File

@ -111,7 +111,7 @@ bool pnkx_local_planner::transformGlobalPlan(
robot_nav_2d_msgs::Pose2DStamped robot_pose; robot_nav_2d_msgs::Pose2DStamped robot_pose;
if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
{ {
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); throw robot_nav_core2::PlannerTFException("transformGlobalPlan: Unable to transform robot pose into global plan's frame");
} }
transformed_plan.header.frame_id = costmap->getGlobalFrameID(); transformed_plan.header.frame_id = costmap->getGlobalFrameID();
@ -172,8 +172,8 @@ bool pnkx_local_planner::transformGlobalPlan(
// Fixed error bound of 1 meter for now. Can reduce to a portion of the map size or based on the resolution // Fixed error bound of 1 meter for now. Can reduce to a portion of the map size or based on the resolution
if (pnkx_local_planner::getSquareDistance(costmap_pose.pose, w.pose) < sq_prune_dist) if (pnkx_local_planner::getSquareDistance(costmap_pose.pose, w.pose) < sq_prune_dist)
{ {
robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n", // robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n",
costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y); // costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y);
break; break;
} }

2
src/Libraries/robot_nav_2d_utils/CMakeLists.txt
View File

@ -37,7 +37,7 @@ if (NOT BUILDING_WITH_CATKIN)
tf3 tf3
robot_tf3_geometry_msgs robot_tf3_geometry_msgs
robot_cpp robot_cpp
console_bridge data_convert
) )
else() else()

12
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
View File

@ -42,6 +42,18 @@
namespace robot_nav_2d_utils namespace robot_nav_2d_utils
{ {
/**
* @brief Transform a PoseStamped from one frame to another
*
* @param tf Smart pointer to TFListener
* @param in_pose Pose to transform
* @param out_pose Place to store the resulting transformed pose
* @param frame Frame to transform the pose into
* @return True if successful transform
*/
bool transform(const TFListenerPtr tf, const ::robot_geometry_msgs::PoseStamped &in_pose,::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame);
/** /**
* @brief Transform a PoseStamped from one frame to another while catching exceptions * @brief Transform a PoseStamped from one frame to another while catching exceptions
* *

4
src/Libraries/robot_nav_2d_utils/package.xml
View File

@ -25,11 +25,13 @@
<build_depend>nav_grid</build_depend> <build_depend>nav_grid</build_depend>
<build_depend>robot_nav_core2</build_depend> <build_depend>robot_nav_core2</build_depend>
<build_depend>robot_cpp</build_depend> <build_depend>robot_cpp</build_depend>
<build_depend>data_convert</build_depend>
<run_depend>robot_nav_2d_msgs</run_depend> <run_depend>robot_nav_2d_msgs</run_depend>
<run_depend>robot_geometry_msgs</run_depend> <run_depend>robot_geometry_msgs</run_depend>
<run_depend>robot_nav_msgs</run_depend> <run_depend>robot_nav_msgs</run_depend>
<run_depend>nav_grid</run_depend> <run_depend>nav_grid</run_depend>
<run_depend>robot_nav_core2</run_depend> <run_depend>robot_nav_core2</run_depend>
<run_depend>robot_cpp</run_depend> <run_depend>robot_cpp</run_depend>
<run_depend>data_convert</run_depend>
</package> </package>

16
src/Libraries/robot_nav_2d_utils/src/conversions.cpp
View File

@ -31,6 +31,8 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <robot/robot.h>
#include <data_convert/data_convert.h>
#include <robot_nav_2d_utils/conversions.h> #include <robot_nav_2d_utils/conversions.h>
#include <vector> #include <vector>
#include <string> #include <string>
@ -105,7 +107,7 @@ namespace robot_nav_2d_utils
pose2d.header = pose.header; pose2d.header = pose.header;
pose2d.pose.x = pose.pose.position.x; pose2d.pose.x = pose.pose.position.x;
pose2d.pose.y = pose.pose.position.y; pose2d.pose.y = pose.pose.position.y;
// pose2d.pose.theta = tf::getYaw(pose.pose.orientation); pose2d.pose.theta = data_convert::getYaw(pose.pose.orientation);
return pose2d; return pose2d;
} }
@ -114,7 +116,7 @@ namespace robot_nav_2d_utils
robot_geometry_msgs::Pose pose; robot_geometry_msgs::Pose pose;
pose.position.x = pose2d.x; pose.position.x = pose2d.x;
pose.position.y = pose2d.y; pose.position.y = pose2d.y;
// pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta);
return pose; return pose;
} }
@ -134,7 +136,7 @@ namespace robot_nav_2d_utils
pose.header.stamp = stamp; pose.header.stamp = stamp;
pose.pose.position.x = pose2d.x; pose.pose.position.x = pose2d.x;
pose.pose.position.y = pose2d.y; pose.pose.position.y = pose2d.y;
// pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta);
return pose; return pose;
} }
@ -284,10 +286,10 @@ namespace robot_nav_2d_utils
info.height = metadata.height; info.height = metadata.height;
info.origin_x = metadata.origin.position.x; info.origin_x = metadata.origin.position.x;
info.origin_y = metadata.origin.position.y; info.origin_y = metadata.origin.position.y;
// if (std::abs(tf::getYaw(metadata.origin.orientation)) > 1e-5) if (std::abs(data_convert::getYaw(metadata.origin.orientation)) > 1e-5)
// { {
// std::cout << "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring" << std::endl; robot::log_warning("[conversions] conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring");
// } }
return info; return info;
} }

68
src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
View File

@ -41,36 +41,54 @@
namespace robot_nav_2d_utils namespace robot_nav_2d_utils
{ {
bool transform(const TFListenerPtr tf,
const ::robot_geometry_msgs::PoseStamped &in_pose,
::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame)
{
const std::string target_frame = frame;
const std::string source_frame = in_pose.header.frame_id;
std::string *error_msg;
if(tf->canTransform(target_frame, source_frame, tf3::Time(), error_msg))
{
tf3::TransformStampedMsg transform =tf->lookupTransform(target_frame, source_frame, tf3::Time());
tf3::doTransform(in_pose, out_pose, transform);
return true;
}
return false;
}
bool transformPose(const TFListenerPtr tf, const ::std::string frame, bool transformPose(const TFListenerPtr tf, const ::std::string frame,
const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose, const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
const bool extrapolation_fallback) const bool extrapolation_fallback)
{ {
// if (in_pose.header.frame_id == frame) if (in_pose.header.frame_id == frame)
// { {
// out_pose = in_pose; out_pose = in_pose;
// return true; return true;
// } }
// try try
// { {
// tf->transform(in_pose, out_pose, frame); transform(tf, in_pose, out_pose, frame);
// return true; return true;
// } }
// catch (tf3::ExtrapolationException &ex) catch (tf3::ExtrapolationException &ex)
// { {
// if (!extrapolation_fallback) if (!extrapolation_fallback)
// throw; throw;
// robot_geometry_msgs::PoseStamped latest_in_pose; robot_geometry_msgs::PoseStamped latest_in_pose;
// latest_in_pose.header.frame_id = in_pose.header.frame_id; latest_in_pose.header.frame_id = in_pose.header.frame_id;
// latest_in_pose.pose = in_pose.pose; latest_in_pose.pose = in_pose.pose;
// tf->transform(latest_in_pose, out_pose, frame); transform(tf, latest_in_pose, out_pose, frame);
// return true; return true;
// } }
// catch (tf3::TransformException &ex) catch (tf3::TransformException &ex)
// { {
// robot::log_error("Exception in transformPose: %s", ex.what()); robot::log_error("Exception in transformPose: %s", ex.what());
// return false; return false;
// } }
return false; return false;
} }

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) 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; return false;
} }
cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity); 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); robot::log_error("Failed to find a plan without Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
return false; return false;
} }
else
{
global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
}
lock.unlock(); lock.unlock();
return true; 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); robot::log_error("Failed to find a plan with Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
return false; return false;
} }
else
{
global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
}
lock.unlock(); lock.unlock();
return true; 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 // if we're controlling, we'll attempt to find valid velocity commands
case move_base::CONTROLLING: 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 // check to see if we've reached our goal
try try
@ -2198,12 +2206,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{ {
if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel)) 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(); last_valid_control_ = robot::Time::now();
// make sure that we send the velocity command to the base // make sure that we send the velocity command to the base
// vel_pub_.publish(cmd_vel);
if (nav_feedback_) if (nav_feedback_)
{ {
@ -2216,8 +2220,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
} }
else else
{ {
robot::log_debug("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::log_warning("The local planner could not find a valid plan.");
robot::Time attempt_end = last_valid_control_ + robot::Duration(controller_patience_); 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 // 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 // enable the planner thread in case it isn't running on a clock
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
// if (as_->isPreemptRequested()) if (as_->isPreemptRequested())
// { {
// if (as_->isNewGoalAvailable()) if (as_->isNewGoalAvailable())
// { {
// // if we're active and a new goal is available, we'll accept it, but we won't shut anything down // 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(); robot_move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
// if (!isQuaternionValid(new_goal.target_pose.pose.orientation)) if (!isQuaternionValid(new_goal.target_pose.pose.orientation))
// { {
// if (nav_feedback_) if (nav_feedback_)
// { {
// nav_feedback_->navigation_state = move_base_core::State::ABORTED; 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"); 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"); as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
// return false; return false;
// } }
// else else
// { {
// if (nav_feedback_) if (nav_feedback_)
// { {
// nav_feedback_->navigation_state = move_base_core::State::ACTIVE; nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE;
// nav_feedback_->feed_back_str = std::string("The new goal is received"); nav_feedback_->feed_back_str = std::string("The new goal is received");
// } }
// } }
// goal = goalToGlobalFrame(new_goal.target_pose); goal = goalToGlobalFrame(new_goal.target_pose);
// planner_goal_ = goal; planner_goal_ = goal;
// } }
// } }
runPlanner_ = true; runPlanner_ = true;
cancel_ctr_ = false; cancel_ctr_ = false;
planner_cond_.notify_one(); planner_cond_.notify_one();
@ -2311,21 +2314,13 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
} }
break; 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: case move_base::CLEARING:
robot::log_debug("[%s:%d] In clearing/recovery state", __FILE__, __LINE__); 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 // we'll invoke whatever recovery behavior we're currently on if they're enabled
if (recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()) 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()); 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(); recovery_behaviors_[recovery_index_]->runBehavior();
// we at least want to give the robot some time to stop oscillating after executing the behavior // 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_->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."); 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) 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_->navigation_state = robot::move_base_core::State::ABORTED;
nav_feedback_->feed_back_str = "Failed to find a valid plan. Even after executing recovery behaviors."; 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) 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_->navigation_state = robot::move_base_core::State::ABORTED;
nav_feedback_->feed_back_str = std::string("Robot is oscillating. Even after executing recovery behaviors."); 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; 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_->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."); 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; return true;
} }