controller_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  * controller_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 
47  const std::string &action_name,
48  const mbf_utility::RobotInformation &robot_info)
49  : AbstractActionBase(action_name, robot_info)
50 {
51 }
52 
54  GoalHandle &goal_handle,
55  typename AbstractControllerExecution::Ptr execution_ptr
56 )
57 {
58  if(goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING)
59  {
60  goal_handle.setCanceled();
61  return;
62  }
63 
64  uint8_t slot = goal_handle.getGoal()->concurrency_slot;
65 
66  bool update_plan = false;
67  slot_map_mtx_.lock();
68  std::map<uint8_t, ConcurrencySlot>::iterator slot_it = concurrency_slots_.find(slot);
69  if(slot_it != concurrency_slots_.end() && slot_it->second.in_use)
70  {
71  boost::lock_guard<boost::mutex> goal_guard(goal_mtx_);
72  if(slot_it->second.execution->getName() == goal_handle.getGoal()->controller ||
73  goal_handle.getGoal()->controller.empty())
74  {
75  update_plan = true;
76  // Goal requests to run the same controller on the same concurrency slot already in use:
77  // we update the goal handle and pass the new plan and tolerances from the action to the
78  // execution without stopping it
79  execution_ptr = slot_it->second.execution;
80  execution_ptr->setNewPlan(goal_handle.getGoal()->path.poses,
81  goal_handle.getGoal()->tolerance_from_action,
82  goal_handle.getGoal()->dist_tolerance,
83  goal_handle.getGoal()->angle_tolerance);
84  // Update also goal pose, so the feedback remains consistent
85  goal_pose_ = goal_handle.getGoal()->path.poses.back();
86  mbf_msgs::ExePathResult result;
87  fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Goal preempted by a new plan", result);
88  concurrency_slots_[slot].goal_handle.setCanceled(result, result.message);
89  concurrency_slots_[slot].goal_handle = goal_handle;
90  concurrency_slots_[slot].goal_handle.setAccepted();
91  }
92  }
93  slot_map_mtx_.unlock();
94  if(!update_plan)
95  {
96  // Otherwise run parent version of this method
97  AbstractActionBase::start(goal_handle, execution_ptr);
98  }
99 }
100 
101 void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecution &execution)
102 {
103  goal_mtx_.lock();
104  // Note that we always use the goal handle stored on the concurrency slots map, as it can change when replanning
105  uint8_t slot = goal_handle.getGoal()->concurrency_slot;
106  goal_mtx_.unlock();
107 
108  ROS_DEBUG_STREAM_NAMED(name_, "Start action " << name_);
109 
110  // ensure we don't provide values from previous execution on case of error before filling both poses
111  goal_pose_ = geometry_msgs::PoseStamped();
112  robot_pose_ = geometry_msgs::PoseStamped();
113 
114  ros::NodeHandle private_nh("~");
115 
116  double oscillation_timeout_tmp;
117  private_nh.param("oscillation_timeout", oscillation_timeout_tmp, 0.0);
118  ros::Duration oscillation_timeout(oscillation_timeout_tmp);
119 
120  double oscillation_distance;
121  private_nh.param("oscillation_distance", oscillation_distance, 0.03);
122 
123  mbf_msgs::ExePathResult result;
124  mbf_msgs::ExePathFeedback feedback;
125 
126  typename AbstractControllerExecution::ControllerState state_moving_input;
127  bool controller_active = true;
128 
129  goal_mtx_.lock();
130  const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
131 
132  const std::vector<geometry_msgs::PoseStamped> &plan = goal.path.poses;
133  if (plan.empty())
134  {
135  fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan!", result);
136  goal_handle.setAborted(result, result.message);
137  ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
138  controller_active = false;
139  goal_mtx_.unlock();
140  return;
141  }
142 
143  goal_pose_ = plan.back();
144  ROS_DEBUG_STREAM_NAMED(name_, "Called action \""
145  << name_ << "\" with plan:" << std::endl
146  << "frame: \"" << goal.path.header.frame_id << "\" " << std::endl
147  << "stamp: " << goal.path.header.stamp << std::endl
148  << "poses: " << goal.path.poses.size() << std::endl
149  << "goal: (" << goal_pose_.pose.position.x << ", "
150  << goal_pose_.pose.position.y << ", "
151  << goal_pose_.pose.position.z << ")");
152 
153  goal_mtx_.unlock();
154 
155 
156  geometry_msgs::PoseStamped oscillation_pose;
157  ros::Time last_oscillation_reset = ros::Time::now();
158 
159  bool first_cycle = true;
160 
161  while (controller_active && ros::ok())
162  {
163  // goal_handle could change between the loop cycles due to adapting the plan
164  // with a new goal received for the same concurrency slot
165  if (!robot_info_.getRobotPose(robot_pose_))
166  {
167  controller_active = false;
168  fillExePathResult(mbf_msgs::ExePathResult::TF_ERROR, "Could not get the robot pose!", result);
169  goal_mtx_.lock();
170  goal_handle.setAborted(result, result.message);
171  goal_mtx_.unlock();
172  ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
173  break;
174  }
175 
176  if (first_cycle)
177  {
178  // init oscillation pose
179  oscillation_pose = robot_pose_;
180  }
181 
182  goal_mtx_.lock();
183  state_moving_input = execution.getState();
184 
185  switch (state_moving_input)
186  {
188  execution.setNewPlan(plan, goal.tolerance_from_action, goal.dist_tolerance, goal.angle_tolerance);
189  execution.start();
190  break;
191 
193  ROS_WARN_STREAM_NAMED(name_, "The controller has been stopped rigorously!");
194  controller_active = false;
195  result.outcome = mbf_msgs::ExePathResult::STOPPED;
196  result.message = "Controller has been stopped!";
197  goal_handle.setAborted(result, result.message);
198  break;
199 
201  ROS_INFO_STREAM("Action \"exe_path\" canceled");
202  fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Controller canceled", result);
203  goal_handle.setCanceled(result, result.message);
204  controller_active = false;
205  break;
206 
208  ROS_DEBUG_STREAM_NAMED(name_, "The moving has been started!");
209  break;
210 
212  if (execution.isPatienceExceeded())
213  {
214  ROS_INFO_STREAM("Try to cancel the plugin \"" << name_ << "\" after the patience time has been exceeded!");
215  if (execution.cancel())
216  {
217  ROS_INFO_STREAM("Successfully canceled the plugin \"" << name_ << "\" after the patience time has been exceeded!");
218  }
219  }
220  break;
221 
223  ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the maximum number of retries!");
224  controller_active = false;
225  fillExePathResult(execution.getOutcome(), execution.getMessage(), result);
226  goal_handle.setAborted(result, result.message);
227  break;
228 
230  ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the patience time");
231  controller_active = false;
232  fillExePathResult(mbf_msgs::ExePathResult::PAT_EXCEEDED, execution.getMessage(), result);
233  goal_handle.setAborted(result, result.message);
234  break;
235 
237  ROS_WARN_STREAM_NAMED(name_, "The controller has been started without a plan!");
238  controller_active = false;
239  fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started without a path", result);
240  goal_handle.setAborted(result, result.message);
241  break;
242 
244  ROS_WARN_STREAM_NAMED(name_, "The controller has received an empty plan");
245  controller_active = false;
246  fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan", result);
247  goal_handle.setAborted(result, result.message);
248  break;
249 
251  ROS_WARN_STREAM_NAMED(name_, "The controller has received an invalid plan");
252  controller_active = false;
253  fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an invalid plan", result);
254  goal_handle.setAborted(result, result.message);
255  break;
256 
258  ROS_WARN_STREAM_THROTTLE_NAMED(3, name_, "No velocity command received from controller! "
259  << execution.getMessage());
260  controller_active = execution.isMoving();
261  if (!controller_active)
262  {
263  fillExePathResult(execution.getOutcome(), execution.getMessage(), result);
264  goal_handle.setAborted(result, result.message);
265  }
266  else
267  {
268  publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(),
269  execution.getVelocityCmd());
270  }
271  break;
272 
274  if (!oscillation_timeout.isZero())
275  {
276  // check if oscillating
277  if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance)
278  {
279  last_oscillation_reset = ros::Time::now();
280  oscillation_pose = robot_pose_;
281  }
282  else if (last_oscillation_reset + oscillation_timeout < ros::Time::now())
283  {
284  ROS_WARN_STREAM_NAMED(name_, "The controller is oscillating for "
285  << (ros::Time::now() - last_oscillation_reset).toSec() << "s");
286 
287  execution.cancel();
288  controller_active = false;
289  fillExePathResult(mbf_msgs::ExePathResult::OSCILLATION, "Oscillation detected!", result);
290  goal_handle.setAborted(result, result.message);
291  break;
292  }
293  }
294  publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd());
295  break;
296 
298  ROS_DEBUG_STREAM_NAMED(name_, "Controller succeeded; arrived at goal");
299  controller_active = false;
300  fillExePathResult(mbf_msgs::ExePathResult::SUCCESS, "Controller succeeded; arrived at goal!", result);
301  goal_handle.setSucceeded(result, result.message);
302  break;
303 
305  ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin: " << execution.getMessage());
306  controller_active = false;
307  fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result);
308  goal_handle.setAborted(result, result.message);
309  break;
310 
311  default:
312  std::stringstream ss;
313  ss << "Internal error: Unknown state in a move base flex controller execution with the number: "
314  << static_cast<int>(state_moving_input);
315  fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, ss.str(), result);
316  ROS_FATAL_STREAM_NAMED(name_, result.message);
317  goal_handle.setAborted(result, result.message);
318  controller_active = false;
319  }
320  goal_mtx_.unlock();
321 
322  if (controller_active)
323  {
324  // try to sleep a bit
325  // normally this thread should be woken up from the controller execution thread
326  // in order to transfer the results to the controller
327  execution.waitForStateUpdate(boost::chrono::milliseconds(500));
328  }
329 
330  first_cycle = false;
331  } // while (controller_active && ros::ok())
332 
333  if (!controller_active)
334  {
335  ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
336  }
337  else
338  {
339  // normal on continuous replanning
340  ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
341  }
342 }
343 
345  GoalHandle &goal_handle,
346  uint32_t outcome, const std::string &message,
347  const geometry_msgs::TwistStamped &current_twist)
348 {
349  mbf_msgs::ExePathFeedback feedback;
350  feedback.outcome = outcome;
351  feedback.message = message;
352 
353  feedback.last_cmd_vel = current_twist;
354  if (feedback.last_cmd_vel.header.stamp.isZero())
355  feedback.last_cmd_vel.header.stamp = ros::Time::now();
356 
357  feedback.current_pose = robot_pose_;
358  feedback.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
359  feedback.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
360  goal_handle.publishFeedback(feedback);
361 }
362 
364  uint32_t outcome, const std::string &message,
365  mbf_msgs::ExePathResult &result)
366 {
367  result.outcome = outcome;
368  result.message = message;
369  result.final_pose = robot_pose_;
370  result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
371  result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
372 }
373 
374 } /* mbf_abstract_nav */
mbf_abstract_nav::AbstractControllerExecution::getVelocityCmd
geometry_msgs::TwistStamped getVelocityCmd() const
Returns the last velocity command calculated by the plugin. Set by setVelocityCmd method....
Definition: src/abstract_controller_execution.cpp:203
ROS_WARN_STREAM_THROTTLE_NAMED
#define ROS_WARN_STREAM_THROTTLE_NAMED(period, name, args)
mbf_abstract_nav::AbstractControllerExecution::MAX_RETRIES
@ MAX_RETRIES
Exceeded the maximum number of retries without a valid command.
Definition: abstract_controller_execution.h:140
mbf_abstract_nav::AbstractControllerExecution::ARRIVED_GOAL
@ ARRIVED_GOAL
The robot arrived the goal.
Definition: abstract_controller_execution.h:146
mbf_abstract_nav::ControllerAction::goal_mtx_
boost::mutex goal_mtx_
lock goal handle for updating it while running
Definition: controller_action.h:94
boost::shared_ptr
mbf_abstract_nav::AbstractControllerExecution::getState
ControllerState getState() const
Return the current state of the controller execution. Thread communication safe.
Definition: src/abstract_controller_execution.cpp:126
mbf_utility::RobotInformation
mbf_utility::distance
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
mbf_abstract_nav::AbstractControllerExecution::PLANNING
@ PLANNING
Executing the plugin.
Definition: abstract_controller_execution.h:138
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
mbf_abstract_nav::AbstractControllerExecution::INVALID_PLAN
@ INVALID_PLAN
Received an invalid plan that the controller plugin rejected.
Definition: abstract_controller_execution.h:143
mbf_abstract_nav::AbstractControllerExecution::ControllerState
ControllerState
Internal states.
Definition: abstract_controller_execution.h:134
mbf_abstract_nav::ControllerAction::goal_pose_
geometry_msgs::PoseStamped goal_pose_
Current goal pose.
Definition: controller_action.h:96
mbf_abstract_nav::ControllerAction::fillExePathResult
void fillExePathResult(uint32_t outcome, const std::string &message, mbf_msgs::ExePathResult &result)
Utility method to fill the ExePath action result in a single line.
Definition: controller_action.cpp:363
mbf_abstract_nav
Definition: abstract_controller_execution.h:58
mbf_abstract_nav::ControllerAction::runImpl
void runImpl(GoalHandle &goal_handle, AbstractControllerExecution &execution)
Definition: controller_action.cpp:101
mbf_abstract_nav::AbstractControllerExecution::NO_PLAN
@ NO_PLAN
The controller has been started without a plan.
Definition: abstract_controller_execution.h:139
mbf_abstract_nav::AbstractExecutionBase::getOutcome
uint32_t getOutcome() const
Gets the current plugin execution outcome.
Definition: src/abstract_execution_base.cpp:89
ros::ok
ROSCPP_DECL bool ok()
mbf_abstract_nav::ControllerAction::publishExePathFeedback
void publishExePathFeedback(GoalHandle &goal_handle, uint32_t outcome, const std::string &message, const geometry_msgs::TwistStamped &current_twist)
Definition: controller_action.cpp:344
mbf_abstract_nav::AbstractControllerExecution::GOT_LOCAL_CMD
@ GOT_LOCAL_CMD
Got a valid velocity command from the plugin.
Definition: abstract_controller_execution.h:145
mbf_abstract_nav::AbstractControllerExecution::INITIALIZED
@ INITIALIZED
Controller has been initialized successfully.
Definition: abstract_controller_execution.h:136
mbf_abstract_nav::ControllerAction::robot_pose_
geometry_msgs::PoseStamped robot_pose_
Current robot pose.
Definition: controller_action.h:95
controller_action.h
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
mbf_abstract_nav::AbstractControllerExecution::INTERNAL_ERROR
@ INTERNAL_ERROR
An internal error occurred.
Definition: abstract_controller_execution.h:149
mbf_utility::angle
double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
mbf_abstract_nav::AbstractControllerExecution::EMPTY_PLAN
@ EMPTY_PLAN
Received an empty plan.
Definition: abstract_controller_execution.h:142
mbf_abstract_nav::AbstractControllerExecution::cancel
virtual bool cancel()
Cancel the controller execution. Normally called upon aborting the navigation. This calls the cancel ...
Definition: src/abstract_controller_execution.cpp:255
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
mbf_abstract_nav::AbstractControllerExecution::start
virtual bool start()
Starts the controller, a valid plan should be given in advance.
Definition: src/abstract_controller_execution.cpp:108
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
mbf_abstract_nav::ControllerAction::start
void start(GoalHandle &goal_handle, typename AbstractControllerExecution::Ptr execution_ptr)
Start controller action. Override abstract action version to allow updating current plan without stop...
Definition: controller_action.cpp:53
mbf_abstract_nav::ControllerAction::ControllerAction
ControllerAction(const std::string &name, const mbf_utility::RobotInformation &robot_info)
Definition: controller_action.cpp:46
ros::Time
mbf_abstract_nav::AbstractExecutionBase::getMessage
const std::string & getMessage() const
Gets the current plugin execution message.
Definition: src/abstract_execution_base.cpp:94
mbf_abstract_nav::AbstractControllerExecution::NO_LOCAL_CMD
@ NO_LOCAL_CMD
Received no velocity command by the plugin, in the current cycle.
Definition: abstract_controller_execution.h:144
mbf_abstract_nav::AbstractControllerExecution::STOPPED
@ STOPPED
The controller has been stopped!
Definition: abstract_controller_execution.h:148
mbf_abstract_nav::AbstractControllerExecution::CANCELED
@ CANCELED
The controller has been canceled.
Definition: abstract_controller_execution.h:147
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
mbf_abstract_nav::AbstractControllerExecution::STARTED
@ STARTED
Controller has been started.
Definition: abstract_controller_execution.h:137
mbf_abstract_nav::AbstractExecutionBase::waitForStateUpdate
boost::cv_status waitForStateUpdate(boost::chrono::microseconds const &duration)
Definition: src/abstract_execution_base.cpp:82
mbf_abstract_nav::AbstractControllerExecution::setNewPlan
void setNewPlan(const std::vector< geometry_msgs::PoseStamped > &plan, bool tolerance_from_action=false, double action_dist_tolerance=1.0, double action_angle_tolerance=3.1415)
Sets a new plan to the controller execution.
Definition: src/abstract_controller_execution.cpp:132
mbf_abstract_nav::AbstractControllerExecution::PAT_EXCEEDED
@ PAT_EXCEEDED
Exceeded the patience time without a valid command.
Definition: abstract_controller_execution.h:141
mbf_abstract_nav::AbstractControllerExecution
The AbstractControllerExecution class loads and binds the controller plugin. It contains a thread run...
Definition: abstract_controller_execution.h:75
ros::Duration
mbf_abstract_nav::AbstractControllerExecution::isMoving
bool isMoving() const
Returns whether the robot should normally move or not. True if the controller seems to work properly.
Definition: src/abstract_controller_execution.cpp:234
DurationBase< Duration >::isZero
bool isZero() const
mbf_abstract_nav::AbstractControllerExecution::isPatienceExceeded
bool isPatienceExceeded() const
Checks whether the patience duration time has been exceeded, ot not.
Definition: src/abstract_controller_execution.cpp:215
ros::NodeHandle
ros::Time::now
static Time now()


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Wed Mar 2 2022 00:33:47