src/planner_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  * planner_action.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 <sstream>
42 
44 
45 namespace mbf_abstract_nav
46 {
47 
49  const std::string &name,
50  const mbf_utility::RobotInformation &robot_info)
51  : AbstractActionBase(name, robot_info), path_seq_count_(0)
52 {
53  ros::NodeHandle private_nh("~");
54  // informative topics: current navigation goal
55  current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
56 }
57 
58 void PlannerAction::runImpl(GoalHandle &goal_handle, AbstractPlannerExecution &execution)
59 {
60  const mbf_msgs::GetPathGoal& goal = *(goal_handle.getGoal().get());
61 
62  mbf_msgs::GetPathResult result;
63  geometry_msgs::PoseStamped start_pose;
64 
65  result.path.header.seq = path_seq_count_++;
66  result.path.header.frame_id = robot_info_.getGlobalFrame();
67 
68  double tolerance = goal.tolerance;
69  bool use_start_pose = goal.use_start_pose;
70  current_goal_pub_.publish(goal.target_pose);
71 
72  bool planner_active = true;
73 
74  if(use_start_pose)
75  {
76  start_pose = goal.start_pose;
77  const geometry_msgs::Point& p = start_pose.pose.position;
78  ROS_DEBUG_STREAM_NAMED(name_, "Use the given start pose (" << p.x << ", " << p.y << ", " << p.z << ").");
79  }
80  else
81  {
82  // get the current robot pose
83  if (!robot_info_.getRobotPose(start_pose))
84  {
85  result.outcome = mbf_msgs::GetPathResult::TF_ERROR;
86  result.message = "Could not get the current robot pose!";
87  goal_handle.setAborted(result, result.message);
88  ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
89  return;
90  }
91  else
92  {
93  const geometry_msgs::Point& p = start_pose.pose.position;
94  ROS_DEBUG_STREAM_NAMED(name_, "Got the current robot pose at ("
95  << p.x << ", " << p.y << ", " << p.z << ").");
96  }
97  }
98 
99  AbstractPlannerExecution::PlanningState state_planning_input;
100 
101  std::vector<geometry_msgs::PoseStamped> plan, global_plan;
102 
103  while (planner_active && ros::ok())
104  {
105  // get the current state of the planning thread
106  state_planning_input = execution.getState();
107 
108  switch (state_planning_input)
109  {
111  ROS_DEBUG_STREAM_NAMED(name_, "planner state: initialized");
112  if (!execution.start(start_pose, goal.target_pose, tolerance))
113  {
114  result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
115  result.message = "Another thread is still planning!";
116  goal_handle.setAborted(result, result.message);
117  ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
118  planner_active = false;
119  }
120  break;
121 
123  ROS_DEBUG_STREAM_NAMED(name_, "planner state: started");
124  break;
125 
127  ROS_DEBUG_STREAM_NAMED(name_, "planner state: stopped");
128  ROS_WARN_STREAM_NAMED(name_, "Planning has been stopped rigorously!");
129  result.outcome = mbf_msgs::GetPathResult::STOPPED;
130  result.message = "Global planner has been stopped!";
131  goal_handle.setAborted(result, result.message);
132  planner_active = false;
133  break;
134 
136  ROS_DEBUG_STREAM_NAMED(name_, "planner state: canceled");
137  ROS_DEBUG_STREAM_NAMED(name_, "Global planner has been canceled successfully");
138  result.path.header.stamp = ros::Time::now();
139  result.outcome = mbf_msgs::GetPathResult::CANCELED;
140  result.message = "Global planner has been canceled!";
141  goal_handle.setCanceled(result, result.message);
142  planner_active = false;
143  break;
144 
145  // in progress
147  if (execution.isPatienceExceeded())
148  {
149  ROS_INFO_STREAM_NAMED(name_, "Global planner patience has been exceeded! Cancel planning...");
150  execution.cancel();
151  }
152  else
153  {
154  ROS_DEBUG_THROTTLE_NAMED(2.0, name_, "planner state: planning");
155  }
156  break;
157 
158  // found a new plan
160  // set time stamp to now
161  result.path.header.stamp = ros::Time::now();
162  plan = execution.getPlan();
163 
164  ROS_DEBUG_STREAM_NAMED(name_, "planner state: found plan with cost: " << execution.getCost());
165 
166  if (!transformPlanToGlobalFrame(plan, global_plan))
167  {
168  result.outcome = mbf_msgs::GetPathResult::TF_ERROR;
169  result.message = "Could not transform the plan to the global frame!";
170 
171  ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
172  goal_handle.setAborted(result, result.message);
173  planner_active = false;
174  break;
175  }
176 
177  if (global_plan.empty())
178  {
179  result.outcome = mbf_msgs::GetPathResult::EMPTY_PATH;
180  result.message = "Global planner returned an empty path!";
181 
182  ROS_ERROR_STREAM_NAMED(name_, result.message);
183  goal_handle.setAborted(result, result.message);
184  planner_active = false;
185  break;
186  }
187 
188  result.path.poses = global_plan;
189  result.cost = execution.getCost();
190  result.outcome = execution.getOutcome();
191  result.message = execution.getMessage();
192  goal_handle.setSucceeded(result, result.message);
193 
194  planner_active = false;
195  break;
196 
197  // no plan found
199  ROS_DEBUG_STREAM_NAMED(name_, "planner state: no plan found");
200  result.outcome = execution.getOutcome();
201  result.message = execution.getMessage();
202  goal_handle.setAborted(result, result.message);
203  planner_active = false;
204  break;
205 
207  ROS_DEBUG_STREAM_NAMED(name_, "Global planner reached the maximum number of retries");
208  result.outcome = execution.getOutcome();
209  result.message = execution.getMessage();
210  goal_handle.setAborted(result, result.message);
211  planner_active = false;
212  break;
213 
215  ROS_DEBUG_STREAM_NAMED(name_, "Global planner exceeded the patience time");
216  result.outcome = mbf_msgs::GetPathResult::PAT_EXCEEDED;
217  result.message = "Global planner exceeded the patience time";
218  goal_handle.setAborted(result, result.message);
219  planner_active = false;
220  break;
221 
223  ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin!"); // TODO getMessage from planning
224  planner_active = false;
225  result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
226  result.message = "Internal error: Unknown error thrown by the plugin!";
227  goal_handle.setAborted(result, result.message);
228  break;
229 
230  default:
231  result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
232  std::ostringstream ss;
233  ss << "Internal error: Unknown state in a move base flex planner execution with the number: "
234  << static_cast<int>(state_planning_input);
235  result.message = ss.str();
236  ROS_FATAL_STREAM_NAMED(name_, result.message);
237  goal_handle.setAborted(result, result.message);
238  planner_active = false;
239  }
240 
241 
242  if (planner_active)
243  {
244  // try to sleep a bit
245  // normally this thread should be woken up from the planner execution thread
246  // in order to transfer the results to the controller.
247  execution.waitForStateUpdate(boost::chrono::milliseconds(500));
248  }
249  } // while (planner_active && ros::ok())
250 
251  if (!planner_active)
252  {
253  ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
254  }
255  else
256  {
257  ROS_ERROR_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
258  }
259 }
260 
261 bool PlannerAction::transformPlanToGlobalFrame(const std::vector<geometry_msgs::PoseStamped>& plan,
262  std::vector<geometry_msgs::PoseStamped>& global_plan)
263 {
264  global_plan.clear();
265  global_plan.reserve(plan.size());
266  std::vector<geometry_msgs::PoseStamped>::const_iterator iter;
267  bool tf_success = false;
268  for (iter = plan.begin(); iter != plan.end(); ++iter)
269  {
270  geometry_msgs::PoseStamped global_pose;
271  tf_success = mbf_utility::transformPose(robot_info_.getTransformListener(), robot_info_.getGlobalFrame(),
272  robot_info_.getTfTimeout(), *iter, global_pose);
273  if (!tf_success)
274  {
275  ROS_ERROR_STREAM("Can not transform pose from the \"" << iter->header.frame_id << "\" frame into the \""
276  << robot_info_.getGlobalFrame() << "\" frame !");
277  return false;
278  }
279  global_plan.push_back(global_pose);
280  }
281  return true;
282 }
283 
284 } /* namespace mbf_abstract_nav */
mbf_abstract_nav::AbstractPlannerExecution::INTERNAL_ERROR
@ INTERNAL_ERROR
An internal error occurred.
Definition: abstract_planner_execution.h:136
ROS_DEBUG_THROTTLE_NAMED
#define ROS_DEBUG_THROTTLE_NAMED(period, name,...)
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
mbf_abstract_nav::AbstractPlannerExecution::start
bool start(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance)
Starts the planner execution thread with the given parameters.
Definition: src/abstract_planner_execution.cpp:207
mbf_utility::RobotInformation
mbf_abstract_nav::PlannerAction::path_seq_count_
unsigned int path_seq_count_
Path sequence counter.
Definition: planner_action.h:86
mbf_abstract_nav::AbstractPlannerExecution::isPatienceExceeded
bool isPatienceExceeded() const
Checks whether the patience was exceeded.
Definition: src/abstract_planner_execution.cpp:163
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
mbf_abstract_nav::PlannerAction::transformPlanToGlobalFrame
bool transformPlanToGlobalFrame(const std::vector< geometry_msgs::PoseStamped > &plan, std::vector< geometry_msgs::PoseStamped > &global_plan)
Transforms a plan to the global frame (global_frame_) coord system.
Definition: src/planner_action.cpp:261
mbf_abstract_nav::AbstractPlannerExecution::PLANNING
@ PLANNING
Executing the plugin.
Definition: abstract_planner_execution.h:129
mbf_abstract_nav
Definition: abstract_controller_execution.h:58
mbf_abstract_nav::AbstractExecutionBase::getOutcome
uint32_t getOutcome() const
Gets the current plugin execution outcome.
Definition: src/abstract_execution_base.cpp:89
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
mbf_abstract_nav::AbstractPlannerExecution::PAT_EXCEEDED
@ PAT_EXCEEDED
Exceeded the patience time without a valid command.
Definition: abstract_planner_execution.h:132
use_start_pose
goal goal use_start_pose
Definition: test/planner_action.cpp:181
mbf_abstract_nav::AbstractPlannerExecution::getCost
double getCost() const
Gets computed costs.
Definition: src/abstract_planner_execution.cpp:110
mbf_abstract_nav::AbstractPlannerExecution::NO_PLAN_FOUND
@ NO_PLAN_FOUND
No plan has been found (MAX_RETRIES and PAT_EXCEEDED are 0).
Definition: abstract_planner_execution.h:133
mbf_abstract_nav::AbstractPlannerExecution::INITIALIZED
@ INITIALIZED
Planner initialized.
Definition: abstract_planner_execution.h:127
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
mbf_utility::transformPose
bool transformPose(const TF &tf, const std::string &target_frame, const ros::Duration &timeout, const geometry_msgs::PoseStamped &in, geometry_msgs::PoseStamped &out)
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
mbf_abstract_nav::PlannerAction::current_goal_pub_
ros::Publisher current_goal_pub_
Publisher to publish the current goal pose, which is used for path planning.
Definition: planner_action.h:83
mbf_abstract_nav::AbstractPlannerExecution::getPlan
std::vector< geometry_msgs::PoseStamped > getPlan() const
Returns a new plan, if one is available.
Definition: src/abstract_planner_execution.cpp:169
mbf_abstract_nav::AbstractPlannerExecution::STOPPED
@ STOPPED
The planner has been stopped.
Definition: abstract_planner_execution.h:135
mbf_abstract_nav::AbstractPlannerExecution::MAX_RETRIES
@ MAX_RETRIES
Exceeded the maximum number of retries without a valid command.
Definition: abstract_planner_execution.h:131
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
mbf_abstract_nav::AbstractPlannerExecution::CANCELED
@ CANCELED
The planner has been canceled.
Definition: abstract_planner_execution.h:134
mbf_abstract_nav::AbstractPlannerExecution::getState
PlanningState getState() const
Returns the current internal state.
Definition: src/abstract_planner_execution.cpp:136
mbf_abstract_nav::AbstractExecutionBase::getMessage
const std::string & getMessage() const
Gets the current plugin execution message.
Definition: src/abstract_execution_base.cpp:94
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
mbf_abstract_nav::AbstractPlannerExecution
The AbstractPlannerExecution class loads and binds the global planner plugin. It contains a thread ru...
Definition: abstract_planner_execution.h:74
mbf_abstract_nav::AbstractPlannerExecution::PlanningState
PlanningState
Internal states.
Definition: abstract_planner_execution.h:125
mbf_abstract_nav::AbstractPlannerExecution::cancel
virtual bool cancel()
Cancel the planner execution. This calls the cancel method of the planner plugin. This could be usefu...
Definition: src/abstract_planner_execution.cpp:231
mbf_abstract_nav::PlannerAction::runImpl
void runImpl(GoalHandle &goal_handle, AbstractPlannerExecution &execution)
Definition: src/planner_action.cpp:58
mbf_abstract_nav::AbstractExecutionBase::waitForStateUpdate
boost::cv_status waitForStateUpdate(boost::chrono::microseconds const &duration)
Definition: src/abstract_execution_base.cpp:82
mbf_abstract_nav::AbstractPlannerExecution::FOUND_PLAN
@ FOUND_PLAN
Found a valid plan.
Definition: abstract_planner_execution.h:130
mbf_abstract_nav::PlannerAction::PlannerAction
PlannerAction(const std::string &name, const mbf_utility::RobotInformation &robot_info)
Definition: src/planner_action.cpp:48
mbf_abstract_nav::AbstractPlannerExecution::STARTED
@ STARTED
Planner started.
Definition: abstract_planner_execution.h:128
ros::NodeHandle
planner_action.h
ros::Time::now
static Time now()


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