abstract_navigation_server.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above
12  * copyright notice, this list of conditions and the following
13  * disclaimer in the documentation and/or other materials provided
14  * with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * abstract_navigation_server.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 
41 #include <nav_msgs/Path.h>
42 
44 
45 namespace mbf_abstract_nav
46 {
47 
49  : tf_listener_ptr_(tf_listener_ptr), private_nh_("~"),
50  planner_plugin_manager_("planners",
51  boost::bind(&AbstractNavigationServer::loadPlannerPlugin, this, _1),
52  boost::bind(&AbstractNavigationServer::initializePlannerPlugin, this, _1, _2)),
53  controller_plugin_manager_("controllers",
54  boost::bind(&AbstractNavigationServer::loadControllerPlugin, this, _1),
55  boost::bind(&AbstractNavigationServer::initializeControllerPlugin, this, _1, _2)),
56  recovery_plugin_manager_("recovery_behaviors",
57  boost::bind(&AbstractNavigationServer::loadRecoveryPlugin, this, _1),
58  boost::bind(&AbstractNavigationServer::initializeRecoveryPlugin, this, _1, _2)),
59  tf_timeout_(private_nh_.param<double>("tf_timeout", 3.0)),
60  global_frame_(private_nh_.param<std::string>("global_frame", "map")),
61  robot_frame_(private_nh_.param<std::string>("robot_frame", "base_link")),
62  robot_info_(*tf_listener_ptr, global_frame_, robot_frame_, tf_timeout_),
63  controller_action_(name_action_exe_path, robot_info_),
64  planner_action_(name_action_get_path, robot_info_),
65  recovery_action_(name_action_recovery, robot_info_),
66  move_base_action_(name_action_move_base, robot_info_, recovery_plugin_manager_.getLoadedNames())
67 {
68  ros::NodeHandle nh;
69 
70  goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
71 
72  // init cmd_vel publisher for the robot velocity
73  vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
74 
81  false));
82 
89  false));
90 
97  false));
98 
101  private_nh_,
105  false));
106 
107  // XXX note that we don't start a dynamic reconfigure server, to avoid colliding with the one possibly created by
108  // the base class. If none, it should call startDynamicReconfigureServer method to start the one defined here for
109  // providing just the abstract server parameters
110 }
111 
113 {
117 }
118 
120 {
121 
122 }
123 
125 {
126  const mbf_msgs::GetPathGoal &goal = *(goal_handle.getGoal().get());
127  const geometry_msgs::Point &p = goal.target_pose.pose.position;
128 
129  std::string planner_name;
131  {
132  planner_name = goal.planner.empty() ? planner_plugin_manager_.getLoadedNames().front() : goal.planner;
133  }
134  else
135  {
136  mbf_msgs::GetPathResult result;
137  result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
138  result.message = "No plugins loaded at all!";
139  ROS_WARN_STREAM_NAMED("get_path", result.message);
140  goal_handle.setRejected(result, result.message);
141  return;
142  }
143 
144  if(!planner_plugin_manager_.hasPlugin(planner_name))
145  {
146  mbf_msgs::GetPathResult result;
147  result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
148  result.message = "No plugin loaded with the given name \"" + goal.planner + "\"!";
149  ROS_WARN_STREAM_NAMED("get_path", result.message);
150  goal_handle.setRejected(result, result.message);
151  return;
152  }
153 
155  ROS_DEBUG_STREAM_NAMED("get_path", "Start action \"get_path\" using planner \"" << planner_name
156  << "\" of type \"" << planner_plugin_manager_.getType(planner_name) << "\"");
157 
158 
159  if(planner_plugin)
160  {
162  = newPlannerExecution(planner_name, planner_plugin);
163 
164  //start another planning action
165  planner_action_.start(goal_handle, planner_execution);
166  }
167  else
168  {
169  mbf_msgs::GetPathResult result;
170  result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
171  result.message = "Internal Error: \"planner_plugin\" pointer should not be a null pointer!";
172  ROS_FATAL_STREAM_NAMED("get_path", result.message);
173  goal_handle.setRejected(result, result.message);
174  }
175 }
176 
178 {
179  ROS_INFO_STREAM_NAMED("get_path", "Cancel action \"get_path\"");
180  planner_action_.cancel(goal_handle);
181 }
182 
184 {
185  const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
186 
187  std::string controller_name;
189  {
190  controller_name = goal.controller.empty() ? controller_plugin_manager_.getLoadedNames().front() : goal.controller;
191  }
192  else
193  {
194  mbf_msgs::ExePathResult result;
195  result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
196  result.message = "No plugins loaded at all!";
197  ROS_WARN_STREAM_NAMED("exe_path", result.message);
198  goal_handle.setRejected(result, result.message);
199  return;
200  }
201 
202  if(!controller_plugin_manager_.hasPlugin(controller_name))
203  {
204  mbf_msgs::ExePathResult result;
205  result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
206  result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!";
207  ROS_WARN_STREAM_NAMED("exe_path", result.message);
208  goal_handle.setRejected(result, result.message);
209  return;
210  }
211 
213  ROS_DEBUG_STREAM_NAMED("exe_path", "Start action \"exe_path\" using controller \"" << controller_name
214  << "\" of type \"" << controller_plugin_manager_.getType(controller_name) << "\"");
215 
216 
217  if(controller_plugin)
218  {
220  = newControllerExecution(controller_name, controller_plugin);
221 
222  // starts another controller action
223  controller_action_.start(goal_handle, controller_execution);
224  }
225  else
226  {
227  mbf_msgs::ExePathResult result;
228  result.outcome = mbf_msgs::ExePathResult::INTERNAL_ERROR;
229  result.message = "Internal Error: \"controller_plugin\" pointer should not be a null pointer!";
230  ROS_FATAL_STREAM_NAMED("exe_path", result.message);
231  goal_handle.setRejected(result, result.message);
232  }
233 }
234 
236 {
237  ROS_INFO_STREAM_NAMED("exe_path", "Cancel action \"exe_path\"");
238  controller_action_.cancel(goal_handle);
239 }
240 
242 {
243  const mbf_msgs::RecoveryGoal &goal = *(goal_handle.getGoal().get());
244 
245  std::string recovery_name;
246 
248  {
249  recovery_name = goal.behavior.empty() ? recovery_plugin_manager_.getLoadedNames().front() : goal.behavior;
250  }
251  else
252  {
253  mbf_msgs::RecoveryResult result;
254  result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
255  result.message = "No plugins loaded at all!";
256  ROS_WARN_STREAM_NAMED("recovery", result.message);
257  goal_handle.setRejected(result, result.message);
258  return;
259  }
260 
261  if(!recovery_plugin_manager_.hasPlugin(recovery_name))
262  {
263  mbf_msgs::RecoveryResult result;
264  result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
265  result.message = "No plugin loaded with the given name \"" + goal.behavior + "\"!";
266  ROS_WARN_STREAM_NAMED("recovery", result.message);
267  goal_handle.setRejected(result, result.message);
268  return;
269  }
270 
272  ROS_DEBUG_STREAM_NAMED("recovery", "Start action \"recovery\" using recovery \"" << recovery_name
273  << "\" of type \"" << recovery_plugin_manager_.getType(recovery_name) << "\"");
274 
275 
276  if(recovery_plugin)
277  {
279  = newRecoveryExecution(recovery_name, recovery_plugin);
280 
281  recovery_action_.start(goal_handle, recovery_execution);
282  }
283  else
284  {
285  mbf_msgs::RecoveryResult result;
286  result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
287  result.message = "Internal Error: \"recovery_plugin\" pointer should not be a null pointer!";
288  ROS_FATAL_STREAM_NAMED("recovery", result.message);
289  goal_handle.setRejected(result, result.message);
290  }
291 }
292 
294 {
295  ROS_INFO_STREAM_NAMED("recovery", "Cancel action \"recovery\"");
296  recovery_action_.cancel(goal_handle);
297 }
298 
300 {
301  ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
302  move_base_action_.start(goal_handle);
303 }
304 
306 {
307  ROS_INFO_STREAM_NAMED("move_base", "Cancel action \"move_base\"");
309 }
310 
312  const std::string &plugin_name,
313  const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr)
314 {
315  return boost::make_shared<mbf_abstract_nav::AbstractPlannerExecution>(plugin_name, plugin_ptr, last_config_);
316 }
317 
319  const std::string &plugin_name,
321 {
322  return boost::make_shared<mbf_abstract_nav::AbstractControllerExecution>(plugin_name, plugin_ptr, vel_pub_, goal_pub_,
324 }
325 
327  const std::string &plugin_name,
329 {
330  return boost::make_shared<mbf_abstract_nav::AbstractRecoveryExecution>(plugin_name, plugin_ptr,
332 }
333 
335 {
340 }
341 
343 {
344  // dynamic reconfigure server
345  dsrv_ = boost::make_shared<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBaseFlexConfig> >(private_nh_);
346  dsrv_->setCallback(boost::bind(&AbstractNavigationServer::reconfigure, this, _1, _2));
347 }
348 
350  mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
351 {
352  boost::lock_guard<boost::mutex> guard(configuration_mutex_);
353 
354  // Make sure we have the original configuration the first time we're called, so we can restore it if needed
355  if (!setup_reconfigure_)
356  {
357  default_config_ = config;
358  setup_reconfigure_ = true;
359  }
360 
361  if (config.restore_defaults)
362  {
363  config = default_config_;
364  // if someone sets restore defaults on the parameter server, prevent looping
365  config.restore_defaults = false;
366  }
367  planner_action_.reconfigureAll(config, level);
368  controller_action_.reconfigureAll(config, level);
369  recovery_action_.reconfigureAll(config, level);
370  move_base_action_.reconfigure(config, level);
371 
372  last_config_ = config;
373 }
374 
376  planner_action_.cancelAll();
377  controller_action_.cancelAll();
378  recovery_action_.cancelAll();
380 }
381 
382 } /* namespace mbf_abstract_nav */
ActionServerRecoveryPtr action_server_recovery_ptr_
shared pointer to the Recovery action server
actionlib::ActionServer< mbf_msgs::MoveBaseAction > ActionServerMoveBase
MoveBase action server.
bool param(const std::string &param_name, T &param_val, const T &default_val)
actionlib::ActionServer< mbf_msgs::RecoveryAction > ActionServerRecovery
Recovery action server.
ros::Publisher goal_pub_
current_goal publisher for all controller execution objects
virtual void cancelActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
const TFPtr tf_listener_ptr_
shared pointer to the common TransformListener
#define ROS_DEBUG_STREAM_NAMED(name, args)
boost::shared_ptr< ActionServerMoveBase > ActionServerMoveBasePtr
PluginType::Ptr getPlugin(const std::string &name)
virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr newPlannerExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr)
Create a new abstract planner execution.
AbstractPluginManager< mbf_abstract_core::AbstractController > controller_plugin_manager_
virtual void startDynamicReconfigureServer()
Start a dynamic reconfigure server. This must be called only if the extending doesn&#39;t create its own...
virtual mbf_abstract_nav::AbstractControllerExecution::Ptr newControllerExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractController::Ptr &plugin_ptr)
Create a new abstract controller execution.
const std::string name_action_get_path
GetPath action topic name.
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
boost::shared_ptr< const Goal > getGoal() const
The AbstractNavigationServer is the abstract base class for all navigation servers in move_base_flex ...
virtual void cancelActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
#define ROS_INFO_STREAM_NAMED(name, args)
virtual void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
Reconfiguration method called by dynamic reconfigure.
actionlib::ActionServer< mbf_msgs::GetPathAction > ActionServerGetPath
GetPath action server.
actionlib::ActionServer< mbf_msgs::ExePathAction > ActionServerExePath
ExePath action server.
virtual void callActionExePath(ActionServerExePath::GoalHandle goal_handle)
ExePath action execution method. This method will be called if the action server receives a goal...
ActionServerGetPathPtr action_server_get_path_ptr_
shared pointer to the GetPath action server
const std::string name_action_exe_path
ExePath action topic name.
const std::vector< std::string > & getLoadedNames()
void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
AbstractPluginManager< mbf_abstract_core::AbstractPlanner > planner_plugin_manager_
bool setup_reconfigure_
true, if the dynamic reconfigure has been setup.
#define ROS_FATAL_STREAM_NAMED(name, args)
mbf_abstract_nav::MoveBaseFlexConfig default_config_
the default parameter configuration save
void start(GoalHandle &goal_handle, typename AbstractControllerExecution::Ptr execution_ptr)
Start controller action. Override abstract action version to allow updating current plan without stop...
virtual void callActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
Recovery action execution method. This method will be called if the action server receives a goal...
AbstractNavigationServer(const TFPtr &tf_listener_ptr)
Constructor, reads all parameters and initializes all action servers and creates the plugin instances...
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher vel_pub_
cmd_vel publisher for all controller execution objects
virtual void cancelActionExePath(ActionServerExePath::GoalHandle goal_handle)
bool hasPlugin(const std::string &name)
boost::shared_ptr< ActionServerGetPath > ActionServerGetPathPtr
virtual void callActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
MoveBase action execution method. This method will be called if the action server receives a goal...
mbf_abstract_nav::MoveBaseFlexConfig last_config_
last configuration save
ActionServerMoveBasePtr action_server_move_base_ptr_
shared pointer to the MoveBase action server
boost::shared_ptr< ActionServerExePath > ActionServerExePathPtr
ActionServerExePathPtr action_server_exe_path_ptr_
shared pointer to the ExePath action server
const std::string name_action_recovery
Recovery action topic name.
virtual void callActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
GetPath action execution method. This method will be called if the action server receives a goal...
boost::mutex configuration_mutex_
configuration mutex for derived classes and other threads.
AbstractPluginManager< mbf_abstract_core::AbstractRecovery > recovery_plugin_manager_
virtual void cancelActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
virtual void startActionServers()
starts all action server.
const std::string name_action_move_base
MoveBase action topic name.
DynamicReconfigureServer dsrv_
dynamic reconfigure server
virtual void initializeServerComponents()
initializes all server components. Initializing the plugins of the Planner, the Controller, and the Recovery Behavior.
virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr newRecoveryExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr)
Create a new abstract recovery behavior execution.
boost::shared_ptr< ActionServerRecovery > ActionServerRecoveryPtr
void start(GoalHandle &goal_handle)
ros::NodeHandle private_nh_
Private node handle.
#define ROS_WARN_STREAM_NAMED(name, args)
std::string getType(const std::string &name)


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Fri Nov 6 2020 03:56:24