abstract_recovery_execution.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_recovery_execution.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 <boost/exception/diagnostic_information.hpp>
42 
44 
45 namespace mbf_abstract_nav
46 {
47 
49  const std::string &name,
50  const mbf_abstract_core::AbstractRecovery::Ptr &recovery_ptr,
51  const TFPtr &tf_listener_ptr,
52  const MoveBaseFlexConfig &config) :
54  behavior_(recovery_ptr), tf_listener_ptr_(tf_listener_ptr), state_(INITIALIZED)
55 {
56  // dynamically reconfigurable parameters
57  reconfigure(config);
58 }
59 
61 {
62 }
63 
64 
65 void AbstractRecoveryExecution::reconfigure(const MoveBaseFlexConfig &config)
66 {
67  boost::lock_guard<boost::mutex> guard(conf_mtx_);
68 
69  // Maximum time allowed to recovery behaviors. Intended as a safeward for the case a behavior hangs.
70  // If it doesn't return within time, the navigator will cancel it and abort the corresponding action.
71  patience_ = ros::Duration(config.recovery_patience);
72 
73  // Nothing else to do here, as recovery_enabled is loaded and used in the navigation server
74 }
75 
76 
78 {
79  boost::lock_guard<boost::mutex> guard(state_mtx_);
80  state_ = state;
81 }
82 
83 
85 {
86  boost::lock_guard<boost::mutex> guard(state_mtx_);
87  return state_;
88 }
89 
91 {
92  cancel_ = true;
93  // returns false if cancel is not implemented or rejected by the recovery behavior (will run until completion)
94  if (!behavior_->cancel())
95  {
96  ROS_WARN_STREAM("Cancel recovery behavior \"" << name_ << "\" failed or is not supported by the plugin. "
97  << "Wait until the current recovery behavior finished!");
98  return false;
99  }
100  return true;
101 }
102 
104 {
105  boost::lock_guard<boost::mutex> guard1(conf_mtx_);
106  boost::lock_guard<boost::mutex> guard2(time_mtx_);
107  ROS_DEBUG_STREAM("Patience: " << patience_ << ", start time: " << start_time_ << " now: " << ros::Time::now());
108  return !patience_.isZero() && (ros::Time::now() - start_time_ > patience_);
109 }
110 
112 {
113  cancel_ = false; // reset the canceled state
114 
115  time_mtx_.lock();
117  time_mtx_.unlock();
119  try
120  {
121  outcome_ = behavior_->runBehavior(message_);
122  if (cancel_)
123  {
125  }
126  else
127  {
129  }
130  }
131  catch (boost::thread_interrupted &ex)
132  {
133  ROS_WARN_STREAM("Recovery \"" << name_ << "\" interrupted!");
134  setState(STOPPED);
135  }
136  catch (...)
137  {
138  ROS_FATAL_STREAM("Unknown error occurred in recovery behavior \"" << name_ << "\": " << boost::current_exception_diagnostic_information());
140  }
141  condition_.notify_one();
142 }
143 
144 } /* namespace mbf_abstract_nav */
boost::condition_variable condition_
condition variable to wake up control thread
void reconfigure(const MoveBaseFlexConfig &config)
Reconfigures the current configuration and reloads all parameters. This method is called from a dynam...
std::string name_
the plugin name; not the plugin type!
AbstractRecoveryExecution::RecoveryState getState()
Returns the current state, thread-safe communication.
bool cancel_
flag for canceling controlling
std::string message_
the last received plugin execution message
void setState(RecoveryState state)
Sets the current internal state. This method is thread communication safe.
#define ROS_FATAL_STREAM(args)
virtual void run()
Main execution method which will be executed by the recovery execution thread_.
uint32_t outcome_
the last received plugin execution outcome
AbstractRecoveryExecution(const std::string &name, const mbf_abstract_core::AbstractRecovery::Ptr &recovery_ptr, const TFPtr &tf_listener_ptr, const MoveBaseFlexConfig &config)
Constructor.
ros::Time start_time_
recovery behavior start time
boost::mutex state_mtx_
mutex to handle safe thread communication for the current state
boost::mutex conf_mtx_
dynamic reconfigure and start time mutexes to mutually exclude read/write configuration ...
ros::Duration patience_
recovery behavior allowed time
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
virtual bool cancel()
Cancel the planner execution. This calls the cancel method of the planner plugin. This could be usefu...
mbf_abstract_core::AbstractRecovery::Ptr behavior_
the current loaded recovery behavior
bool isPatienceExceeded()
Checks whether the patience was exceeded.
static Time now()


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