recovery_action.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  * recovery_action.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 
42 
43 namespace mbf_abstract_nav
44 {
45 
46 RecoveryAction::RecoveryAction(const std::string &name, const mbf_utility::RobotInformation &robot_info)
47  : AbstractActionBase(name, robot_info, boost::bind(&mbf_abstract_nav::RecoveryAction::run, this, _1, _2)){}
48 
49 void RecoveryAction::run(GoalHandle &goal_handle, AbstractRecoveryExecution &execution)
50 {
51  ROS_DEBUG_STREAM_NAMED(name_, "Start action " << name_);
52 
53  const mbf_msgs::RecoveryGoal &goal = *goal_handle.getGoal();
54  mbf_msgs::RecoveryResult result;
55  result.used_plugin = goal.behavior;
56  bool recovery_active = true;
57 
58  typename AbstractRecoveryExecution::RecoveryState state_recovery_input;
59 
60  while (recovery_active && ros::ok())
61  {
62  state_recovery_input = execution.getState();
63  switch (state_recovery_input)
64  {
66  ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior \"" << goal.behavior << "\" initialized.");
67  execution.start();
68  break;
69 
71  ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior stopped rigorously");
72  result.outcome = mbf_msgs::RecoveryResult::STOPPED;
73  result.message = "Recovery has been stopped!";
74  goal_handle.setAborted(result, result.message);
75  recovery_active = false;
76  break;
77 
79  ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior \"" << goal.behavior << "\" was started");
80  break;
81 
83 
84  if (execution.isPatienceExceeded())
85  {
86  ROS_INFO_STREAM("Recovery behavior \"" << goal.behavior << "\" patience exceeded! Cancel recovering...");
87  execution.cancel();
88  }
89 
90  ROS_DEBUG_STREAM_THROTTLE_NAMED(3, name_, "Recovering with: " << goal.behavior);
91  break;
92 
94  // Recovery behavior supports cancel and it worked
95  recovery_active = false; // stopping the action
96  result.outcome = mbf_msgs::RecoveryResult::CANCELED;
97  result.message = "Recovery behaviour \"" + goal.behavior + "\" canceled!";
98  goal_handle.setCanceled(result, result.message);
99  ROS_DEBUG_STREAM_NAMED(name_, result.message);
100  break;
101 
103  recovery_active = false; // stopping the action
104  result.outcome = execution.getOutcome();
105  result.message = execution.getMessage();
106  if (result.message.empty())
107  {
108  if (result.outcome < 10)
109  result.message = "Recovery \"" + goal.behavior + "\" done";
110  else
111  result.message = "Recovery \"" + goal.behavior + "\" FAILED";
112  }
113 
114  ROS_DEBUG_STREAM_NAMED(name_, result.message);
115  goal_handle.setSucceeded(result, result.message);
116  break;
117 
119  ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin!"); // TODO getMessage from recovery
120  recovery_active = false;
121  result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
122  result.message = "Internal error: Unknown error thrown by the plugin!";
123  goal_handle.setAborted(result, result.message);
124  break;
125 
126  default:
127  result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
128  std::stringstream ss;
129  ss << "Internal error: Unknown state in a move base flex recovery execution with the number: "
130  << static_cast<int>(state_recovery_input);
131  result.message = ss.str();
132  ROS_FATAL_STREAM_NAMED(name_, result.message);
133  goal_handle.setAborted(result, result.message);
134  recovery_active = false;
135  }
136 
137  if (recovery_active)
138  {
139  // try to sleep a bit
140  // normally the thread should be woken up from the recovery unit
141  // in order to transfer the results to the controller
142  execution.waitForStateUpdate(boost::chrono::milliseconds(500));
143  }
144  } // while (recovery_active && ros::ok())
145 
146  if (!recovery_active)
147  {
148  ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
149  }
150  else
151  {
152  ROS_ERROR_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
153  }
154 }
155 
156 } /* namespace mbf_abstract_nav */
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
RecoveryAction(const std::string &name, const mbf_utility::RobotInformation &robot_info)
#define ROS_DEBUG_STREAM_THROTTLE_NAMED(rate, name, args)
void run(GoalHandle &goal_handle, AbstractRecoveryExecution &execution)
AbstractRecoveryExecution::RecoveryState getState()
Returns the current state, thread-safe communication.
boost::cv_status waitForStateUpdate(boost::chrono::microseconds const &duration)
std::string getMessage()
Gets the current plugin execution message.
#define ROS_FATAL_STREAM_NAMED(name, args)
ROSCPP_DECL bool ok()
virtual bool cancel()
Cancel the planner execution. This calls the cancel method of the planner plugin. This could be usefu...
The AbstractRecoveryExecution class loads and binds the recovery behavior plugin. It contains a threa...
#define ROS_INFO_STREAM(args)
bool isPatienceExceeded()
Checks whether the patience was exceeded.
The recovery execution thread has been started.
uint32_t getOutcome()
Gets the current plugin execution outcome.


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