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
00039
00040
00041 #include "mbf_simple_nav/simple_navigation_server.h"
00042
00043 namespace mbf_simple_nav
00044 {
00045
00046 SimpleNavigationServer::SimpleNavigationServer(const TFPtr &tf_listener_ptr) :
00047 mbf_abstract_nav::AbstractNavigationServer(tf_listener_ptr),
00048 planner_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractPlanner"),
00049 controller_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractController"),
00050 recovery_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractRecovery")
00051 {
00052
00053 initializeServerComponents();
00054
00055
00056 startActionServers();
00057 }
00058
00059 mbf_abstract_core::AbstractPlanner::Ptr SimpleNavigationServer::loadPlannerPlugin(const std::string& planner_type)
00060 {
00061 mbf_abstract_core::AbstractPlanner::Ptr planner_ptr;
00062 ROS_INFO("Load global planner plugin.");
00063 try
00064 {
00065 planner_ptr = planner_plugin_loader_.createInstance(planner_type);
00066 }
00067 catch (const pluginlib::PluginlibException &ex)
00068 {
00069 ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it is properly registered"
00070 << " and that the containing library is built? Exception: " << ex.what());
00071 }
00072 ROS_INFO("Global planner plugin loaded.");
00073
00074 return planner_ptr;
00075 }
00076
00077 bool SimpleNavigationServer::initializePlannerPlugin(
00078 const std::string& name,
00079 const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr
00080 )
00081 {
00082 return true;
00083 }
00084
00085
00086 mbf_abstract_core::AbstractController::Ptr SimpleNavigationServer::loadControllerPlugin(
00087 const std::string& controller_type)
00088 {
00089 mbf_abstract_core::AbstractController::Ptr controller_ptr;
00090 ROS_DEBUG("Load controller plugin.");
00091 try
00092 {
00093 controller_ptr = controller_plugin_loader_.createInstance(controller_type);
00094 ROS_INFO_STREAM("MBF_core-based local planner plugin " << controller_type << " loaded");
00095 }
00096 catch (const pluginlib::PluginlibException &ex)
00097 {
00098 ROS_FATAL_STREAM("Failed to load the " << controller_type
00099 << " local planner, are you sure it's properly registered"
00100 << " and that the containing library is built? Exception: " << ex.what());
00101 }
00102 return controller_ptr;
00103 }
00104
00105 bool SimpleNavigationServer::initializeControllerPlugin(
00106 const std::string& name,
00107 const mbf_abstract_core::AbstractController::Ptr& controller_ptr)
00108 {
00109 return true;
00110 }
00111
00112 mbf_abstract_core::AbstractRecovery::Ptr SimpleNavigationServer::loadRecoveryPlugin(
00113 const std::string& recovery_type)
00114 {
00115 mbf_abstract_core::AbstractRecovery::Ptr recovery_ptr;
00116
00117 try
00118 {
00119 recovery_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractRecovery>(
00120 recovery_plugin_loader_.createInstance(recovery_type));
00121 }
00122 catch (pluginlib::PluginlibException &ex)
00123 {
00124 ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered"
00125 << " and that the containing library is built? Exception: " << ex.what());
00126 }
00127 return recovery_ptr;
00128 }
00129
00130 bool SimpleNavigationServer::initializeRecoveryPlugin(
00131 const std::string& name,
00132 const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr)
00133 {
00134 return true;
00135 }
00136
00137
00138 SimpleNavigationServer::~SimpleNavigationServer()
00139 {
00140 }
00141
00142 }