Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <base_local_planner/local_planner_util.h>
00039
00040 #include <base_local_planner/goal_functions.h>
00041
00042 namespace base_local_planner {
00043
00044 void LocalPlannerUtil::initialize(
00045 tf::TransformListener* tf,
00046 costmap_2d::Costmap2D* costmap,
00047 std::string global_frame) {
00048 if(!initialized_) {
00049 tf_ = tf;
00050 costmap_ = costmap;
00051 global_frame_ = global_frame;
00052 initialized_ = true;
00053 }
00054 else{
00055 ROS_WARN("Planner utils have already been initialized, doing nothing.");
00056 }
00057 }
00058
00059 void LocalPlannerUtil::reconfigureCB(LocalPlannerLimits &config, bool restore_defaults)
00060 {
00061 if(setup_ && restore_defaults) {
00062 config = default_limits_;
00063 }
00064
00065 if(!setup_) {
00066 default_limits_ = config;
00067 setup_ = true;
00068 }
00069 boost::mutex::scoped_lock l(limits_configuration_mutex_);
00070 limits_ = LocalPlannerLimits(config);
00071 }
00072
00073 costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() {
00074 return costmap_;
00075 }
00076
00077 LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() {
00078 boost::mutex::scoped_lock l(limits_configuration_mutex_);
00079 return limits_;
00080 }
00081
00082
00083 bool LocalPlannerUtil::getGoal(tf::Stamped<tf::Pose>& goal_pose) {
00084
00085 return base_local_planner::getGoalPose(*tf_,
00086 global_plan_,
00087 global_frame_,
00088 goal_pose);
00089 }
00090
00091 bool LocalPlannerUtil::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
00092 if(!initialized_){
00093 ROS_ERROR("Planner utils have not been initialized, please call initialize() first");
00094 return false;
00095 }
00096
00097
00098 global_plan_.clear();
00099
00100 global_plan_ = orig_global_plan;
00101
00102 return true;
00103 }
00104
00105 bool LocalPlannerUtil::getLocalPlan(tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan) {
00106
00107 if(!base_local_planner::transformGlobalPlan(
00108 *tf_,
00109 global_plan_,
00110 global_pose,
00111 *costmap_,
00112 global_frame_,
00113 transformed_plan)) {
00114 ROS_WARN("Could not transform the global plan to the frame of the controller");
00115 return false;
00116 }
00117
00118
00119 if(limits_.prune_plan) {
00120 base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
00121 }
00122 return true;
00123 }
00124
00125
00126
00127
00128 }