move_base_action.h
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  * move_base_action.h
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 #ifndef MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_
41 #define MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_
42 
45 
46 #include <mbf_msgs/MoveBaseAction.h>
47 #include <mbf_msgs/GetPathAction.h>
48 #include <mbf_msgs/ExePathAction.h>
49 #include <mbf_msgs/RecoveryAction.h>
50 
52 
53 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
54 
55 
56 namespace mbf_abstract_nav
57 {
58 
60 {
61  public:
62 
67 
69 
70  MoveBaseAction(const std::string &name,
71  const mbf_utility::RobotInformation &robot_info,
72  const std::vector<std::string> &controllers);
73 
75 
76  void start(GoalHandle &goal_handle);
77 
78  void cancel();
79 
80  void reconfigure(
81  mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level);
82 
83  protected:
84 
85  void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback);
86 
87  void actionGetPathDone(
89  const mbf_msgs::GetPathResultConstPtr &result);
90 
91  void actionExePathActive();
92 
93  void actionExePathDone(
95  const mbf_msgs::ExePathResultConstPtr &result);
96 
97  void actionRecoveryDone(
99  const mbf_msgs::RecoveryResultConstPtr &result);
100 
101  bool attemptRecovery();
102 
103  bool replanningActive() const;
104 
105  void replanningThread();
106 
113  template <typename ResultType>
114  void fillMoveBaseResult(const ResultType& result, mbf_msgs::MoveBaseResult& move_base_result)
115  {
116  // copy outcome and message from action client result
117  move_base_result.outcome = result.outcome;
118  move_base_result.message = result.message;
119  move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
120  move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
121  move_base_result.final_pose = robot_pose_;
122  }
123 
124  mbf_msgs::ExePathGoal exe_path_goal_;
125  mbf_msgs::GetPathGoal get_path_goal_;
126  mbf_msgs::RecoveryGoal recovery_goal_;
127 
128  geometry_msgs::PoseStamped last_oscillation_pose_;
130 
133 
136 
137  GoalHandle goal_handle_;
138 
139  std::string name_;
140 
143 
145  geometry_msgs::PoseStamped robot_pose_;
146 
148  geometry_msgs::PoseStamped goal_pose_;
149 
151 
153  ActionClientExePath action_client_exe_path_;
154 
156  ActionClientGetPath action_client_get_path_;
157 
159  ActionClientRecovery action_client_recovery_;
160 
163 
166 
168  boost::thread replanning_thread_;
169 
172 
173  std::vector<std::string> recovery_behaviors_;
174 
175  std::vector<std::string>::iterator current_recovery_behavior_;
176 
177  const std::vector<std::string> &behaviors_;
178 
180  {
189  };
190 
193 };
194 
195 } /* mbf_abstract_nav */
196 
197 #endif //MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_
actionlib::SimpleActionClient< mbf_msgs::ExePathAction > ActionClientExePath
double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
const mbf_utility::RobotInformation & robot_info_
current robot state
std::vector< std::string > recovery_behaviors_
mbf_msgs::GetPathGoal get_path_goal_
double dist_to_goal_
current distance to goal (we will stop replanning if very close to avoid destabilizing the controller...
ros::Duration oscillation_timeout_
timeout after a oscillation is detected
geometry_msgs::PoseStamped last_oscillation_pose_
void actionExePathDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result)
boost::thread replanning_thread_
Replanning thread, running permanently.
MoveBaseAction(const std::string &name, const mbf_utility::RobotInformation &robot_info, const std::vector< std::string > &controllers)
ActionClientGetPath action_client_get_path_
Action client used by the move_base action.
void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
const std::vector< std::string > & behaviors_
mbf_msgs::RecoveryGoal recovery_goal_
void actionRecoveryDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::RecoveryResultConstPtr &result)
actionlib::SimpleActionClient< mbf_msgs::RecoveryAction > ActionClientRecovery
void actionGetPathDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::GetPathResultConstPtr &result)
void fillMoveBaseResult(const ResultType &result, mbf_msgs::MoveBaseResult &move_base_result)
mbf_msgs::ExePathGoal exe_path_goal_
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
MoveBaseActionState recovery_trigger_
actionlib::SimpleActionClient< mbf_msgs::GetPathAction > ActionClientGetPath
Action clients for the MoveBase action.
bool recovery_enabled_
true, if recovery behavior for the MoveBase action is enabled.
geometry_msgs::PoseStamped goal_pose_
current goal pose; used to compute remaining distance and angle
actionlib::ActionServer< mbf_msgs::MoveBaseAction >::GoalHandle GoalHandle
std::vector< std::string >::iterator current_recovery_behavior_
geometry_msgs::PoseStamped robot_pose_
current robot pose; updated with exe_path action feedback
ActionClientRecovery action_client_recovery_
Action client used by the move_base action.
void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback)
ros::Duration replanning_period_
Replanning period dynamically reconfigurable.
ActionClientExePath action_client_exe_path_
Action client used by the move_base action.
void start(GoalHandle &goal_handle)
double oscillation_distance_
minimal move distance to not detect an oscillation


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Mon Feb 28 2022 22:49:50