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

update

Browse Source
This commit is contained in:
HiepLM 2026-01-14 10:26:27 +07:00
parent 57b77ac14b
commit 6549425279
2 changed files with 220 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@ -66,10 +66,13 @@ namespace move_base
public:
/**
* @brief Constructor for the actions
* @param name The name of the action
* @param tf A reference to a TransformListener
*/
MoveBase(robot::TFListenerPtr tf);
/**
* @brief Default constructor for the actions
*/
MoveBase();
/**
@ -78,7 +81,9 @@ namespace move_base
*/
virtual void initialize(robot::TFListenerPtr tf) override;
/**
* @brief Initialize the cost-to-occupancy lookup table for costmap conversion
*/
static void initializeCostToOccupancyLUT();
/**
@ -423,8 +428,8 @@ namespace move_base
void setYawGoalTolerance(double yaw_goal_tolerance);
/**
* @brief Set velocity for yaw goal tolerance of the robot
* @param xy_goal_tolerance the value command
* @brief Set the XY goal tolerance for the robot
* @param xy_goal_tolerance The XY tolerance value in meters
*/
void setXyGoalTolerance(double xy_goal_tolerance);
@ -442,6 +447,15 @@ namespace move_base
*/
bool makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);
/**
* @brief Make a new global plan
* @param msg The order message
* @param goal The goal to plan to
* @param plan Will be filled in with the plan made by the planner
* @return True if planning succeeds, false otherwise
*/
bool makePlan(const robot_protocol_msgs::Order &msg, const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);
/**
* @brief Load the recovery behaviors for the navigation stack from the parameter server
* @return True if the recovery behaviors were loaded successfully, false otherwise
@ -470,10 +484,16 @@ namespace move_base
*/
void resetState();
// void goalCB(const robot_geometry_msgs::PoseStamped &goal);
/**
* @brief Thread function that continuously plans paths to the goal
*/
void planThread();
/**
* @brief Callback function to execute a move_base goal
* @param move_base_goal The goal message to execute
*/
void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
/**
@ -481,12 +501,34 @@ namespace move_base
*/
void setupActionServerCallbacks();
/**
* @brief Check if a quaternion is valid (normalized)
* @param q The quaternion to validate
* @return True if the quaternion is valid, false otherwise
*/
bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
/**
* @brief Get the robot's pose in the global frame using a specific costmap
* @param global_pose Output parameter with the robot's current pose
* @param costmap The costmap to use for pose lookup
* @return True if pose was successfully retrieved
*/
bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap);
/**
* @brief Calculate the Euclidean distance between two poses
* @param p1 First pose
* @param p2 Second pose
* @return The distance between the two poses in meters
*/
double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);
/**
* @brief Transform a goal pose to the global frame
* @param goal_pose_msg The goal pose to transform
* @return The transformed pose in the global frame
*/
robot_geometry_msgs::PoseStamped goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg);
/**
@ -495,72 +537,155 @@ namespace move_base
void wakePlanner(const robot::TimerEvent &event);
private:
/** @brief Flag indicating if the move_base has been initialized */
bool initialized_;
/** @brief Transform listener for coordinate frame transformations */
robot::TFListenerPtr tf_;
/** @brief Private node handle for ROS parameters and topics */
robot::NodeHandle private_nh_;
/** @brief Action server for move_base goals */
MoveBaseActionServer *as_;
/** @brief Function loader for global planner plugins */
boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
/** @brief Function loader for local planner (controller) plugins */
boost::function<robot_nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
/** @brief Map of recovery behavior loaders by name */
std::map<std::string, boost::function<robot_nav_core::RecoveryBehavior::Ptr()>> recovery_loaders_;
/** @brief Local planner (trajectory controller) instance */
robot_nav_core::BaseLocalPlanner::Ptr tc_;
/** @brief Global planner instance */
robot_nav_core::BaseGlobalPlanner::Ptr planner_;
/** @brief Vector of recovery behavior instances */
std::vector<robot_nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
/** @brief Costmap for the local planner/controller */
robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
/** @brief Costmap for the global planner */
robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;
/** @brief Robot base frame name and global frame name */
std::string robot_base_frame_, global_frame_;
/** @brief Names of recovery behaviors to use */
std::vector<std::string> recovery_behavior_names_;
/** @brief Current index in the recovery behavior list */
unsigned int recovery_index_;
/** @brief Flag indicating if recovery behaviors are enabled */
bool recovery_behavior_enabled_;
/** @brief Current global pose of the robot */
robot_geometry_msgs::PoseStamped global_pose_;
double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
double planner_patience_, controller_patience_;
/** @brief Frequency for planner execution (Hz) */
double planner_frequency_;
/** @brief Frequency for controller execution (Hz) */
double controller_frequency_;
/** @brief Inscribed radius of the robot footprint (meters) */
double inscribed_radius_;
/** @brief Circumscribed radius of the robot footprint (meters) */
double circumscribed_radius_;
/** @brief Maximum time to wait for a valid plan before triggering recovery (seconds) */
double planner_patience_;
/** @brief Maximum time to wait for valid control before triggering recovery (seconds) */
double controller_patience_;
/** @brief Maximum number of planning retries before giving up */
int32_t max_planning_retries_;
/** @brief Current number of planning retries */
uint32_t planning_retries_;
double conservative_reset_dist_, clearing_radius_;
bool shutdown_costmaps_, clearing_rotation_allowed_;
bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
double oscillation_timeout_, oscillation_distance_;
/** @brief Conservative distance threshold for resetting costmaps (meters) */
double conservative_reset_dist_;
/** @brief Radius around robot to clear obstacles during recovery (meters) */
double clearing_radius_;
/** @brief Flag to shutdown costmaps when not in use */
bool shutdown_costmaps_;
/** @brief Flag allowing rotation during costmap clearing */
bool clearing_rotation_allowed_;
/** @brief Flag to clear costmap when making a new plan */
bool make_plan_clear_costmap_;
/** @brief Flag to add unreachable goals to costmap */
bool make_plan_add_unreachable_goal_;
/** @brief Time threshold for detecting oscillation (seconds) */
double oscillation_timeout_;
/** @brief Distance threshold for detecting oscillation (meters) */
double oscillation_distance_;
/** @brief Current state of the move_base (PLANNING, CONTROLLING, CLEARING) */
MoveBaseState state_;
/** @brief Trigger that caused recovery behavior (PLANNING_R, CONTROLLING_R, OSCILLATION_R) */
RecoveryTrigger recovery_trigger_;
robot::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
/** @brief Timestamp of the last valid plan */
robot::Time last_valid_plan_;
/** @brief Timestamp of the last valid control command */
robot::Time last_valid_control_;
/** @brief Timestamp of the last oscillation reset */
robot::Time last_oscillation_reset_;
/** @brief Pose used for oscillation detection */
robot_geometry_msgs::PoseStamped oscillation_pose_;
// set up plan triple buffer
/** @brief Plan triple buffer: plan from the planner thread */
std::vector<robot_geometry_msgs::PoseStamped> *planner_plan_;
/** @brief Plan triple buffer: latest plan (intermediate buffer) */
std::vector<robot_geometry_msgs::PoseStamped> *latest_plan_;
/** @brief Plan triple buffer: plan used by the controller */
std::vector<robot_geometry_msgs::PoseStamped> *controller_plan_;
// set up the planner's thread
/** @brief Flag to control planner thread execution */
bool runPlanner_;
/** @brief Mutex for protecting planner thread data */
boost::recursive_mutex planner_mutex_;
/** @brief Condition variable to wake planner thread */
boost::condition_variable_any planner_cond_;
/** @brief Goal pose for the planner thread */
robot_geometry_msgs::PoseStamped planner_goal_;
/** @brief Order message for the planner thread (nullptr if not using Order-based planning) */
boost::shared_ptr<robot_protocol_msgs::Order> planner_order_;
/** @brief Thread handle for the planner */
boost::thread *planner_thread_;
/** @brief Mutex for protecting configuration changes */
boost::recursive_mutex configuration_mutex_;
/** @brief Last configuration used */
move_base::MoveBaseConfig last_config_;
/** @brief Default configuration */
move_base::MoveBaseConfig default_config_;
bool setup_, p_freq_change_, c_freq_change_;
/** @brief Flag indicating if setup is complete */
bool setup_;
/** @brief Flag indicating planner frequency changed */
bool p_freq_change_;
/** @brief Flag indicating controller frequency changed */
bool c_freq_change_;
/** @brief Flag indicating a new global plan is available */
bool new_global_plan_;
/** @brief Flag indicating cancel control request */
bool cancel_ctr_;
bool pause_ctr_, paused_;
/** @brief Flag indicating pause control request */
bool pause_ctr_;
/** @brief Flag indicating if robot is currently paused */
bool paused_;
/** @brief Minimum linear velocity when approaching goal (m/s) */
double min_approach_linear_velocity_{0.1};
robot_geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
robot_geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
/** @brief Previous forward linear velocity command */
robot_geometry_msgs::Vector3 old_linear_fwd_;
/** @brief Previous backward linear velocity command */
robot_geometry_msgs::Vector3 old_linear_bwd_;
/** @brief Previous forward angular velocity command */
robot_geometry_msgs::Vector3 old_angular_fwd_;
/** @brief Previous reverse angular velocity command */
robot_geometry_msgs::Vector3 old_angular_rev_;
/** @brief Original XY goal tolerance value (meters) */
double original_xy_goal_tolerance_;
/** @brief Original yaw goal tolerance value (radians) */
double original_yaw_goal_tolerance_;
// defined planner name
/** @brief Name of the position/local planner plugin */
std::string position_planner_name_{"MKTLocalPlanner"};
/** @brief Name of the docking planner plugin */
std::string docking_planner_name_{"NAVDockingLocalPlanner"};
/** @brief Name of the go-straight planner plugin */
std::string go_straight_planner_name_{"NAVGoStraightLocalPlanner"};
/** @brief Name of the rotate planner plugin */
std::string rotate_planner_name_{"NAVRotateLocalPlanner"};
};

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

@ -721,6 +721,13 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
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");
@ -808,6 +815,14 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
// Store Order message for use in planThread
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
robot::log_info("[MoveBase::moveTo] Stored Order message for planning");
}
as_->processGoal(action_goal);
lock.unlock();
@ -1209,7 +1224,7 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
// since this gets called on handle activate
if (!planner_costmap_robot_)
{
std::cerr << "Planner costmap ROS is NULL, unable to create global plan" << std::endl;
robot::log_error("Planner costmap ROS is NULL, unable to create global plan");
return false;
}
@ -1219,7 +1234,7 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
robot_geometry_msgs::PoseStamped global_pose;
if (!getRobotPose(global_pose, planner_costmap_robot_))
{
std::cerr << "Unable to get starting pose of robot, unable to create global plan" << std::endl;
robot::log_error("Unable to get starting pose of robot, unable to create global plan");
return false;
}
@ -1228,13 +1243,46 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
// if the planner fails or returns a zero length plan, planning failed
if (!planner_->makePlan(start, goal, plan) || plan.empty())
{
std::cout << "Failed to find a plan to point (" << goal.pose.position.x << ", " << goal.pose.position.y << ")" << std::endl;
robot::log_error("Failed to find a plan without Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
return false;
}
lock.unlock();
return true;
}
bool move_base::MoveBase::makePlan(const robot_protocol_msgs::Order &msg, const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
// make sure to set the plan to be empty initially
plan.clear();
// since this gets called on handle activate
if (!planner_costmap_robot_)
{
robot::log_error("Planner costmap ROS is NULL, unable to create global plan");
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
// get the starting pose of the robot
robot_geometry_msgs::PoseStamped global_pose;
if (!getRobotPose(global_pose, planner_costmap_robot_))
{
robot::log_error("Unable to get starting pose of robot, unable to create global plan");
return false;
}
const robot_geometry_msgs::PoseStamped &start = global_pose;
// if the planner fails or returns a zero length plan, planning failed
if (!planner_->makePlan(msg, start, goal, plan) || plan.empty())
{
robot::log_error("Failed to find a plan with Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
return false;
}
lock.unlock();
return true;
}
bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
{
YAML::Node behavior_list = node.getParamValue("recovery_behaviors");
@ -1506,7 +1554,6 @@ void move_base::MoveBase::wakePlanner(const robot::TimerEvent &event)
planner_cond_.notify_one();
}
// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);
void move_base::MoveBase::planThread()
{
robot::Timer timer;
@ -1524,14 +1571,29 @@ void move_base::MoveBase::planThread()
}
robot::Time start_time = robot::Time::now();
// time to plan! get a copy of the goal and unlock the mutex
// time to plan! get a copy of the goal and Order message (if available) and unlock the mutex
robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
boost::shared_ptr<robot_protocol_msgs::Order> temp_order = planner_order_;
lock.unlock();
std::cout << "Planning..." << std::endl;
// run planner
planner_plan_->clear();
// ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y);
bool gotPlan = makePlan(temp_goal, *planner_plan_);
// Choose makePlan based on whether Order message is available
bool gotPlan = false;
if (temp_order)
{
// Use Order-based makePlan
robot::log_info("[MoveBase::planThread] Using Order-based planning");
gotPlan = makePlan(*temp_order, temp_goal, *planner_plan_);
}
else
{
// Use standard makePlan without Order
robot::log_info("[MoveBase::planThread] Using standard planning (no Order)");
gotPlan = makePlan(temp_goal, *planner_plan_);
}
if (gotPlan)
{
@ -1602,8 +1664,6 @@ void move_base::MoveBase::planThread()
}
}
void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal)
{
if (!isQuaternionValid(move_base_goal->target_pose.pose.orientation))
@ -1691,6 +1751,8 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
// we have a new goal so make sure the planner is awake
lock.lock();
planner_goal_ = goal;
// Clear Order message for new goal (will be set if moveTo with Order is called)
planner_order_.reset();
runPlanner_ = true;
cancel_ctr_ = false;
planner_cond_.notify_one();
@ -1736,6 +1798,8 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
// we have a new goal so make sure the planner is awake
lock.lock();
planner_goal_ = goal;
// Clear Order message for new goal (will be set if moveTo with Order is called)
planner_order_.reset();
runPlanner_ = true;
cancel_ctr_ = false;
planner_cond_.notify_one();
@ -1822,6 +1886,9 @@ void move_base::MoveBase::setupActionServerCallbacks()
[this](const robot_actionlib_msgs::GoalIDConstPtr& cancel)
{
robot::log_info("[MoveBase] Cancel callback: id=%s", cancel->id.c_str());
// Clear Order message for new goal (will be set if moveTo with Order is called)
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
);