ROS move_base 自主重规划全局路径
众所周知，move_base中的global_planner是可以以一定频率重新规划的，频率可以在move_base的参数planner_frequency中设定。
当我们把planner_frequency的值设为0时，就可以实现全局路径一直不改变，直到局部规划器报错时才重新规划。
最近做探索未知地图导航的时候发现一个问题：如果planner_frequency的值较高，在遇见岔路时全局路径会一直在跳变，导致机器人一会往左转，一会往右转。而在起点处初始速度很慢，宏观上体现出来就是机器人卡在原地动不了。
例如在这个场景下，由于右边那条路未完全探索出来（实际上走不通），全局路径会一直跳变。因此我们要将全局路径frequency改小。
然而，如果调到0，也就是只有局部规划无法满足时才重新全局规划，这个时候机器人很有可能已经陷入无法恢复的状态，甚至可能已经撞墙了。因此，我们需要一个接口，能实现自主重新全局规划。具体思路是：以当前位置为起点向前看一个horizion的距离，当horizon距离内有不合适的路径时重新规划。这个操作实际上是预判了局部规划的报错。
在movebase源码中进行如下修改：
move_base.h：
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV_MOVE_BASE_ACTION_H_
#define NAV_MOVE_BASE_ACTION_H_
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "move_base/MoveBaseConfig.h"
#include
namespace move_base {
//typedefs to help us out with the action server so that we don't hace to type so much
typedef actionlib::SimpleActionServer MoveBaseActionServer;
enum MoveBaseState {
PLANNING,
CONTROLLING,
CLEARING
};
enum RecoveryTrigger
{
PLANNING_R,
CONTROLLING_R,
OSCILLATION_R
};
/**
* @class MoveBase
* @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location.
*/
class MoveBase {
public:
/**
* @brief Constructor for the actions
* @param name The name of the action
* @param tf A reference to a TransformListener
*/
MoveBase(tf2_ros::Buffer& tf);
/**
* @brief Destructor - Cleans up
*/
virtual ~MoveBase();
/**
* @brief Performs a control cycle
* @param goal A reference to the goal to pursue
* @param global_plan A reference to the global plan being used
* @return True if processing of the goal is done, false otherwise
*/
bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector& global_plan);
private:
/**
* @brief A service call that clears the costmaps of obstacles
* @param req The service request
* @param resp The service response
* @return True if the service call succeeds, false otherwise
*/
bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
/**
* @brief A service call that can be made when the action is inactive that will return a plan
* @param req The goal request
* @param resp The plan request
* @return True if planning succeeded, false otherwise
*/
bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
/**
* @brief Make a new global plan
* @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 geometry_msgs::PoseStamped& goal, std::vector& plan);
/**
* @brief Load the recovery behaviors for the navigation stack from the parameter server
* @param node The ros::NodeHandle to be used for loading parameters
* @return True if the recovery behaviors were loaded successfully, false otherwise
*/
bool loadRecoveryBehaviors(ros::NodeHandle node);
/**
* @brief Loads the default recovery behaviors for the navigation stack
*/
void loadDefaultRecoveryBehaviors();
/**
* @brief Clears obstacles within a window around the robot
* @param size_x The x size of the window
* @param size_y The y size of the window
*/
void clearCostmapWindows(double size_x, double size_y);
/**
* @brief Publishes a velocity command of zero to the base
*/
void publishZeroVelocity();
/**
* @brief Reset the state of the move_base action and send a zero velocity command to the base
*/
void resetState();
void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
void planThread();
void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
bool isQuaternionValid(const geometry_msgs::Quaternion& q);
bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap);
double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
/**
* @brief This is used to wake the planner at periodic intervals.
*/
void wakePlanner(const ros::TimerEvent& event);
tf2_ros::Buffer& tf_;
MoveBaseActionServer* as_;
boost::shared_ptr tc_;
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
boost::shared_ptr planner_;
std::string robot_base_frame_, global_frame_;
std::vector > recovery_behaviors_;
std::vector recovery_behavior_names_;
unsigned int recovery_index_;
geometry_msgs::PoseStamped global_pose_;
double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
double planner_patience_, controller_patience_;
int32_t max_planning_retries_;
uint32_t planning_retries_;
double conservative_reset_dist_, clearing_radius_;
ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
ros::Subscriber goal_sub_;
ros::Subscriber horizon_sub_;
ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
double oscillation_timeout_, oscillation_distance_;
MoveBaseState state_;
RecoveryTrigger recovery_trigger_;
ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
geometry_msgs::PoseStamped oscillation_pose_;
pluginlib::ClassLoader bgp_loader_;
pluginlib::ClassLoader blp_loader_;
pluginlib::ClassLoader recovery_loader_;
//set up plan triple buffer
std::vector* planner_plan_;
std::vector* latest_plan_;
std::vector* controller_plan_;
//set up the planner's thread
bool runPlanner_;
boost::recursive_mutex planner_mutex_;
boost::condition_variable_any planner_cond_;
geometry_msgs::PoseStamped planner_goal_;
boost::thread* planner_thread_;
boost::recursive_mutex configuration_mutex_;
dynamic_reconfigure::Server *dsrv_;
void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
move_base::MoveBaseConfig last_config_;
move_base::MoveBaseConfig default_config_;
bool setup_, p_freq_change_, c_freq_change_;
bool new_global_plan_;
void horizonCB(const std_msgs::Bool::ConstPtr& horizon);
};
bool horizon_flag;
};
#endif
加入一个叫horizon_sub_的subscribe
horizon_sub_ = simple_nh.subscribe("horizon", 1, boost::bind(&MoveBase::horizonCB, this, _1)); horizonCB函数：
void MoveBase::horizonCB(const std_msgs::Bool::ConstPtr& horizon){
horizon_flag=horizon->data;
std::cout<<"The current global path is not feasible, plan again!"< lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
} 测试：
import rospy
from std_msgs.msg import Bool
def main():
rospy.init_node("horizon_publisher", anonymous=True)
horizon_publisher = rospy.Publisher('/move_base_simple/horizon', Bool, queue_size=1)
r = rospy.Rate(5) # define rate here
while not rospy.is_shutdown():
msg=Bool()
msg.data=True
horizon_publisher.publish(msg)
r.sleep()
if __name__ == '__main__':
main()可以发现，不运行horizon_publisher时全局路径一直不变，运行后以rate=5Hz进行改变。后续可以根据自己的判断条件publish一个msg让其重新规划。