diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index a48e455..8ce9909 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -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 &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 &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 planner_loader_; + /** @brief Function loader for local planner (controller) plugins */ boost::function controller_loader_; + /** @brief Map of recovery behavior loaders by name */ std::map> 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 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 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 *planner_plan_; + /** @brief Plan triple buffer: latest plan (intermediate buffer) */ std::vector *latest_plan_; + /** @brief Plan triple buffer: plan used by the controller */ std::vector *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 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"}; }; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 5fe0cac..36b4457 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -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 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 planner_lock(planner_mutex_); + planner_order_ = boost::make_shared(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 &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 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 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 planner_lock(planner_mutex_); + planner_order_.reset(); } );