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
00039
00040
00041 #include <nav_msgs/Path.h>
00042 #include <sstream>
00043 #include "mbf_abstract_nav/abstract_navigation_server.h"
00044
00045 namespace mbf_abstract_nav
00046 {
00047
00048 AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
00049 : tf_listener_ptr_(tf_listener_ptr), private_nh_("~"),
00050 planner_plugin_manager_("planners",
00051 boost::bind(&AbstractNavigationServer::loadPlannerPlugin, this, _1),
00052 boost::bind(&AbstractNavigationServer::initializePlannerPlugin, this, _1, _2)),
00053 controller_plugin_manager_("controllers",
00054 boost::bind(&AbstractNavigationServer::loadControllerPlugin, this, _1),
00055 boost::bind(&AbstractNavigationServer::initializeControllerPlugin, this, _1, _2)),
00056 recovery_plugin_manager_("recovery_behaviors",
00057 boost::bind(&AbstractNavigationServer::loadRecoveryPlugin, this, _1),
00058 boost::bind(&AbstractNavigationServer::initializeRecoveryPlugin, this, _1, _2)),
00059 tf_timeout_(private_nh_.param<double>("tf_timeout", 3.0)),
00060 global_frame_(private_nh_.param<std::string>("global_frame", "map")),
00061 robot_frame_(private_nh_.param<std::string>("robot_frame", "base_link")),
00062 robot_info_(*tf_listener_ptr, global_frame_, robot_frame_, tf_timeout_),
00063 controller_action_(name_action_exe_path, robot_info_),
00064 planner_action_(name_action_get_path, robot_info_),
00065 recovery_action_(name_action_recovery, robot_info_),
00066 move_base_action_(name_action_move_base, robot_info_, recovery_plugin_manager_.getLoadedNames())
00067 {
00068 ros::NodeHandle nh;
00069
00070
00071 double oscillation_timeout;
00072 private_nh_.param("oscillation_timeout", oscillation_timeout, 0.0);
00073 oscillation_timeout_ = ros::Duration(oscillation_timeout);
00074 private_nh_.param("oscillation_distance", oscillation_distance_, 0.02);
00075
00076 goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
00077
00078
00079 vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00080
00081 action_server_get_path_ptr_ = ActionServerGetPathPtr(
00082 new ActionServerGetPath(
00083 private_nh_,
00084 name_action_get_path,
00085 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionGetPath, this, _1),
00086 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionGetPath, this, _1),
00087 false));
00088
00089 action_server_exe_path_ptr_ = ActionServerExePathPtr(
00090 new ActionServerExePath(
00091 private_nh_,
00092 name_action_exe_path,
00093 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionExePath, this, _1),
00094 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionExePath, this, _1),
00095 false));
00096
00097 action_server_recovery_ptr_ = ActionServerRecoveryPtr(
00098 new ActionServerRecovery(
00099 private_nh_,
00100 name_action_recovery,
00101 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionRecovery, this, _1),
00102 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionRecovery, this, _1),
00103 false));
00104
00105 action_server_move_base_ptr_ = ActionServerMoveBasePtr(
00106 new ActionServerMoveBase(
00107 private_nh_,
00108 name_action_move_base,
00109 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionMoveBase, this, _1),
00110 boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionMoveBase, this, _1),
00111 false));
00112
00113
00114
00115
00116 }
00117
00118 void AbstractNavigationServer::initializeServerComponents()
00119 {
00120 planner_plugin_manager_.loadPlugins();
00121 controller_plugin_manager_.loadPlugins();
00122 recovery_plugin_manager_.loadPlugins();
00123 }
00124
00125 AbstractNavigationServer::~AbstractNavigationServer()
00126 {
00127
00128 }
00129
00130 void AbstractNavigationServer::callActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
00131 {
00132 const mbf_msgs::GetPathGoal &goal = *(goal_handle.getGoal().get());
00133 const geometry_msgs::Point &p = goal.target_pose.pose.position;
00134
00135 std::string planner_name;
00136 if(!planner_plugin_manager_.getLoadedNames().empty())
00137 {
00138 planner_name = goal.planner.empty() ? planner_plugin_manager_.getLoadedNames().front() : goal.planner;
00139 }
00140 else
00141 {
00142 mbf_msgs::GetPathResult result;
00143 result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
00144 result.message = "No plugins loaded at all!";
00145 ROS_WARN_STREAM_NAMED("get_path", result.message);
00146 goal_handle.setRejected(result, result.message);
00147 return;
00148 }
00149
00150 if(!planner_plugin_manager_.hasPlugin(planner_name))
00151 {
00152 mbf_msgs::GetPathResult result;
00153 result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
00154 result.message = "No plugin loaded with the given name \"" + goal.planner + "\"!";
00155 ROS_WARN_STREAM_NAMED("get_path", result.message);
00156 goal_handle.setRejected(result, result.message);
00157 return;
00158 }
00159
00160 mbf_abstract_core::AbstractPlanner::Ptr planner_plugin = planner_plugin_manager_.getPlugin(planner_name);
00161 ROS_INFO_STREAM_NAMED("get_path", "Start action \"get_path\" using planner \"" << planner_name
00162 << "\" of type \"" << planner_plugin_manager_.getType(planner_name) << "\"");
00163
00164
00165 if(planner_plugin)
00166 {
00167 mbf_abstract_nav::AbstractPlannerExecution::Ptr planner_execution
00168 = newPlannerExecution(planner_name, planner_plugin);
00169
00170
00171 planner_action_.start(goal_handle, planner_execution);
00172 }
00173 else
00174 {
00175 mbf_msgs::GetPathResult result;
00176 result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
00177 result.message = "Internal Error: \"planner_plugin\" pointer should not be a null pointer!";
00178 ROS_FATAL_STREAM_NAMED("get_path", result.message);
00179 goal_handle.setRejected(result, result.message);
00180 }
00181 }
00182
00183 void AbstractNavigationServer::cancelActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
00184 {
00185 ROS_INFO_STREAM_NAMED("get_path", "Cancel action \"get_path\"");
00186 planner_action_.cancel(goal_handle);
00187 }
00188
00189 void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle goal_handle)
00190 {
00191 const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
00192
00193 std::string controller_name;
00194 if(!controller_plugin_manager_.getLoadedNames().empty())
00195 {
00196 controller_name = goal.controller.empty() ? controller_plugin_manager_.getLoadedNames().front() : goal.controller;
00197 }
00198 else
00199 {
00200 mbf_msgs::ExePathResult result;
00201 result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
00202 result.message = "No plugins loaded at all!";
00203 ROS_WARN_STREAM_NAMED("exe_path", result.message);
00204 goal_handle.setRejected(result, result.message);
00205 return;
00206 }
00207
00208 if(!controller_plugin_manager_.hasPlugin(controller_name))
00209 {
00210 mbf_msgs::ExePathResult result;
00211 result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
00212 result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!";
00213 ROS_WARN_STREAM_NAMED("exe_path", result.message);
00214 goal_handle.setRejected(result, result.message);
00215 return;
00216 }
00217
00218 mbf_abstract_core::AbstractController::Ptr controller_plugin = controller_plugin_manager_.getPlugin(controller_name);
00219 ROS_INFO_STREAM_NAMED("exe_path", "Start action \"exe_path\" using controller \"" << controller_name
00220 << "\" of type \"" << controller_plugin_manager_.getType(controller_name) << "\"");
00221
00222
00223 if(controller_plugin)
00224 {
00225 mbf_abstract_nav::AbstractControllerExecution::Ptr controller_execution
00226 = newControllerExecution(controller_name, controller_plugin);
00227
00228
00229 controller_action_.start(goal_handle, controller_execution);
00230 }
00231 else
00232 {
00233 mbf_msgs::ExePathResult result;
00234 result.outcome = mbf_msgs::ExePathResult::INTERNAL_ERROR;
00235 result.message = "Internal Error: \"controller_plugin\" pointer should not be a null pointer!";
00236 ROS_FATAL_STREAM_NAMED("exe_path", result.message);
00237 goal_handle.setRejected(result, result.message);
00238 }
00239 }
00240
00241 void AbstractNavigationServer::cancelActionExePath(ActionServerExePath::GoalHandle goal_handle)
00242 {
00243 ROS_INFO_STREAM_NAMED("exe_path", "Cancel action \"exe_path\"");
00244 controller_action_.cancel(goal_handle);
00245 }
00246
00247 void AbstractNavigationServer::callActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
00248 {
00249 const mbf_msgs::RecoveryGoal &goal = *(goal_handle.getGoal().get());
00250
00251 std::string recovery_name;
00252
00253 if(!recovery_plugin_manager_.getLoadedNames().empty())
00254 {
00255 recovery_name = goal.behavior.empty() ? recovery_plugin_manager_.getLoadedNames().front() : goal.behavior;
00256 }
00257 else
00258 {
00259 mbf_msgs::RecoveryResult result;
00260 result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
00261 result.message = "No plugins loaded at all!";
00262 ROS_WARN_STREAM_NAMED("recovery", result.message);
00263 goal_handle.setRejected(result, result.message);
00264 return;
00265 }
00266
00267 if(!recovery_plugin_manager_.hasPlugin(recovery_name))
00268 {
00269 mbf_msgs::RecoveryResult result;
00270 result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
00271 result.message = "No plugin loaded with the given name \"" + goal.behavior + "\"!";
00272 ROS_WARN_STREAM_NAMED("recovery", result.message);
00273 goal_handle.setRejected(result, result.message);
00274 return;
00275 }
00276
00277 mbf_abstract_core::AbstractRecovery::Ptr recovery_plugin = recovery_plugin_manager_.getPlugin(recovery_name);
00278 ROS_INFO_STREAM_NAMED("recovery", "Start action \"recovery\" using recovery \"" << recovery_name
00279 << "\" of type \"" << recovery_plugin_manager_.getType(recovery_name) << "\"");
00280
00281
00282 if(recovery_plugin)
00283 {
00284 mbf_abstract_nav::AbstractRecoveryExecution::Ptr recovery_execution
00285 = newRecoveryExecution(recovery_name, recovery_plugin);
00286
00287 recovery_action_.start(goal_handle, recovery_execution);
00288 }
00289 else
00290 {
00291 mbf_msgs::RecoveryResult result;
00292 result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
00293 result.message = "Internal Error: \"recovery_plugin\" pointer should not be a null pointer!";
00294 ROS_FATAL_STREAM_NAMED("recovery", result.message);
00295 goal_handle.setRejected(result, result.message);
00296 }
00297 }
00298
00299 void AbstractNavigationServer::cancelActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
00300 {
00301 ROS_INFO_STREAM_NAMED("recovery", "Cancel action \"recovery\"");
00302 recovery_action_.cancel(goal_handle);
00303 }
00304
00305 void AbstractNavigationServer::callActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
00306 {
00307 ROS_INFO_STREAM_NAMED("move_base", "Start action \"move_base\"");
00308 move_base_action_.start(goal_handle);
00309 }
00310
00311 void AbstractNavigationServer::cancelActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
00312 {
00313 ROS_INFO_STREAM_NAMED("move_base", "Cancel action \"move_base\"");
00314 move_base_action_.cancel();
00315 }
00316
00317 mbf_abstract_nav::AbstractPlannerExecution::Ptr AbstractNavigationServer::newPlannerExecution(
00318 const std::string name,
00319 const mbf_abstract_core::AbstractPlanner::Ptr plugin_ptr)
00320 {
00321 return boost::make_shared<mbf_abstract_nav::AbstractPlannerExecution>(name, plugin_ptr, last_config_,
00322 boost::function<void()>(),
00323 boost::function<void()>());
00324 }
00325
00326 mbf_abstract_nav::AbstractControllerExecution::Ptr AbstractNavigationServer::newControllerExecution(
00327 const std::string name,
00328 const mbf_abstract_core::AbstractController::Ptr plugin_ptr)
00329 {
00330 return boost::make_shared<mbf_abstract_nav::AbstractControllerExecution>(name, plugin_ptr, vel_pub_, goal_pub_,
00331 tf_listener_ptr_, last_config_, boost::function<void()>(), boost::function<void()>());
00332 }
00333
00334 mbf_abstract_nav::AbstractRecoveryExecution::Ptr AbstractNavigationServer::newRecoveryExecution(
00335 const std::string name,
00336 const mbf_abstract_core::AbstractRecovery::Ptr plugin_ptr)
00337 {
00338 return boost::make_shared<mbf_abstract_nav::AbstractRecoveryExecution>(name, plugin_ptr, tf_listener_ptr_, last_config_,
00339 boost::function<void()>(),
00340 boost::function<void()>());
00341 }
00342
00343 void AbstractNavigationServer::startActionServers()
00344 {
00345 action_server_get_path_ptr_->start();
00346 action_server_exe_path_ptr_->start();
00347 action_server_recovery_ptr_->start();
00348 action_server_move_base_ptr_->start();
00349 }
00350
00351 void AbstractNavigationServer::startDynamicReconfigureServer()
00352 {
00353
00354 dsrv_ = boost::make_shared<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBaseFlexConfig> >(private_nh_);
00355 dsrv_->setCallback(boost::bind(&AbstractNavigationServer::reconfigure, this, _1, _2));
00356 }
00357
00358 void AbstractNavigationServer::reconfigure(
00359 mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
00360 {
00361 boost::lock_guard<boost::mutex> guard(configuration_mutex_);
00362
00363
00364 if (!setup_reconfigure_)
00365 {
00366 default_config_ = config;
00367 setup_reconfigure_ = true;
00368 }
00369
00370 if (config.restore_defaults)
00371 {
00372 config = default_config_;
00373
00374 config.restore_defaults = false;
00375 }
00376 planner_action_.reconfigureAll(config, level);
00377 controller_action_.reconfigureAll(config, level);
00378 recovery_action_.reconfigureAll(config, level);
00379 move_base_action_.reconfigure(config, level);
00380
00381 last_config_ = config;
00382 }
00383
00384 void AbstractNavigationServer::stop(){
00385 planner_action_.cancelAll();
00386 controller_action_.cancelAll();
00387 recovery_action_.cancelAll();
00388 move_base_action_.cancel();
00389 }
00390
00391 }