abstract_controller_execution.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  * abstract_controller_execution.h
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 
41 #ifndef MBF_ABSTRACT_NAV__ABSTRACT_CONTROLLER_EXECUTION_H_
42 #define MBF_ABSTRACT_NAV__ABSTRACT_CONTROLLER_EXECUTION_H_
43 
44 #include <map>
45 #include <string>
46 #include <vector>
47 
48 #include <geometry_msgs/PoseStamped.h>
49 #include <geometry_msgs/Twist.h>
50 
53 #include <mbf_utility/types.h>
54 
55 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
57 
59 {
60 
76  {
77  public:
78 
79  static const double DEFAULT_CONTROLLER_FREQUENCY;
80 
82 
90  const std::string &name,
91  const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
92  const ros::Publisher &vel_pub,
93  const ros::Publisher &goal_pub,
94  const TFPtr &tf_listener_ptr,
95  const MoveBaseFlexConfig &config);
96 
101 
106  virtual bool start();
107 
115  void setNewPlan(
116  const std::vector<geometry_msgs::PoseStamped> &plan,
117  bool tolerance_from_action = false,
118  double action_dist_tolerance = 1.0,
119  double action_angle_tolerance = 3.1415);
120 
127  virtual bool cancel();
128 
133  {
148  };
149 
155 
161 
168  geometry_msgs::TwistStamped getVelocityCmd();
169 
174  bool isPatienceExceeded();
175 
181  bool setControllerFrequency(double frequency);
182 
188  void reconfigure(const MoveBaseFlexConfig &config);
189 
194  bool isMoving();
195 
196  protected:
197 
208  virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped &pose,
209  const geometry_msgs::TwistStamped &velocity,
210  geometry_msgs::TwistStamped &vel_cmd, std::string &message);
211 
216  void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
217 
219  std::string plugin_name_;
220 
223 
226 
229 
232 
235 
238 
241 
245  virtual void run();
246 
254  virtual bool safetyCheck() { return true; };
255 
256  private:
257 
258 
262  void publishZeroVelocity();
263 
268  bool reachedGoalCheck();
269 
274  bool computeRobotPose();
275 
280  void setState(ControllerState state);
281 
283  boost::mutex state_mtx_;
284 
286  boost::mutex plan_mtx_;
287 
289  boost::mutex vel_cmd_mtx_;
290 
292  boost::mutex lct_mtx_;
293 
295  bool new_plan_;
296 
301  bool hasNewPlan();
302 
307  std::vector<geometry_msgs::PoseStamped> getNewPlan();
308 
310  geometry_msgs::TwistStamped vel_cmd_stamped_;
311 
313  std::vector<geometry_msgs::PoseStamped> plan_;
314 
316  boost::chrono::microseconds calling_duration_;
317 
319  std::string robot_frame_;
320 
322  std::string global_frame_;
323 
326 
329 
332 
334  double tf_timeout_;
335 
337  boost::mutex configuration_mutex_;
338 
340  bool moving_;
341 
344 
347 
350 
353 
356 
358  geometry_msgs::PoseStamped robot_pose_;
359 
362 
365 
368  };
369 
370 } /* namespace mbf_abstract_nav */
371 
372 #endif /* MBF_ABSTRACT_NAV__ABSTRACT_CONTROLLER_EXECUTION_H_ */
boost::mutex plan_mtx_
mutex to handle safe thread communication for the current plan
bool new_plan_
true, if a new plan is available. See hasNewPlan()!
bool tolerance_from_action_
whether check for action specific tolerance
std::string global_frame_
the global frame the robot is controlling in.
boost::chrono::microseconds calling_duration_
the duration which corresponds with the controller frequency.
ros::Duration patience_
The time / duration of patience, before changing the state.
boost::mutex configuration_mutex_
dynamic reconfigure config mutex, thread safe param reading and writing
boost::mutex vel_cmd_mtx_
mutex to handle safe thread communication for the current velocity command
Exceeded the maximum number of retries without a valid command.
Received no velocity command by the plugin, in the current cycle.
ros::Publisher current_goal_pub_
publisher for the current goal
bool hasNewPlan()
Returns true if a new plan is available, false otherwise! A new plan is set by another thread! ...
void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped)
Sets the velocity command, to make it available for another thread.
double dist_tolerance_
distance tolerance to the given goal pose
The controller has been started without a plan.
double action_dist_tolerance_
replaces parameter dist_tolerance_ for the action
void setState(ControllerState state)
Sets the controller state. This method makes the communication of the state thread safe...
The AbstractControllerExecution class loads and binds the controller plugin. It contains a thread run...
ros::Publisher vel_pub_
publisher for the current velocity command
const TFPtr & tf_listener_ptr
shared pointer to the shared tf listener
virtual bool cancel()
Cancel the controller execution. This calls the cancel method of the controller plugin, sets the cancel_ flag to true, and waits for the control loop to stop. Normally called upon aborting the navigation.
virtual void run()
The main run method, a thread will execute this method. It contains the main controller execution loo...
boost::shared_ptr< AbstractControllerExecution > Ptr
geometry_msgs::TwistStamped vel_cmd_stamped_
the last calculated velocity command
Exceeded the patience time without a valid command.
bool isMoving()
Returns whether the robot should normally move or not. True if the controller seems to work properly...
bool mbf_tolerance_check_
whether move base flex should check for the goal tolerance or not.
AbstractControllerExecution::ControllerState state_
the current controller state
bool moving_
main controller loop variable, true if the controller is running, false otherwise ...
std::string plugin_name_
the name of the loaded plugin
bool force_stop_at_goal_
whether move base flex should force the robot to stop once the goal is reached.
bool isPatienceExceeded()
Checks whether the patience duration time has been exceeded, ot not.
double angle_tolerance_
angle tolerance to the given goal pose
virtual bool safetyCheck()
Check if its safe to drive. This method gets called at every controller cycle, stopping the robot if ...
boost::mutex state_mtx_
mutex to handle safe thread communication for the current value of the state
bool force_stop_on_cancel_
whether move base flex should force the robot to stop on canceling navigation.
ros::Time getLastPluginCallTime()
Returns the time of the last plugin call.
ros::Time start_time_
The time the controller has been started.
AbstractControllerExecution(const std::string &name, const mbf_abstract_core::AbstractController::Ptr &controller_ptr, const ros::Publisher &vel_pub, const ros::Publisher &goal_pub, const TFPtr &tf_listener_ptr, const MoveBaseFlexConfig &config)
Constructor.
std::vector< geometry_msgs::PoseStamped > getNewPlan()
Gets the new available plan. This method is thread safe.
virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped &pose, const geometry_msgs::TwistStamped &velocity, geometry_msgs::TwistStamped &vel_cmd, std::string &message)
Request plugin for a new velocity command, given the current position, orientation, and velocity of the robot. We use this virtual method to give concrete implementations as move_base the chance to override it and do additional stuff, for example locking the costmap.
double action_angle_tolerance_
replaces parameter angle_tolerance_ for the action
Received an invalid plan that the controller plugin rejected.
geometry_msgs::TwistStamped getVelocityCmd()
Returns the last velocity command calculated by the plugin. Set by setVelocityCmd method...
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.
void publishZeroVelocity()
Publishes a velocity command with zero values to stop the robot.
bool reachedGoalCheck()
Checks whether the goal has been reached in the range of tolerance or not.
mbf_abstract_core::AbstractController::Ptr controller_
the local planer to calculate the velocity command
ControllerState getState()
Return the current state of the controller execution. Thread communication safe.
boost::mutex lct_mtx_
mutex to handle safe thread communication for the last plugin call time
geometry_msgs::PoseStamped robot_pose_
current robot pose;
std::string robot_frame_
the frame of the robot, which will be used to determine its position.
void reconfigure(const MoveBaseFlexConfig &config)
Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconf...
virtual bool start()
Starts the controller, a valid plan should be given in advance.
ros::Time last_valid_cmd_time_
The time the controller responded with a success output (output < 10).
double tf_timeout_
time before a timeout used for tf requests
bool setControllerFrequency(double frequency)
Sets the controller frequency.
std::vector< geometry_msgs::PoseStamped > plan_
the last set plan which is currently processed by the controller
ros::Time last_call_time_
The current cycle start time of the last cycle run. Will by updated each cycle.


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Thu Dec 3 2020 04:08:43