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 <stdint.h>
46 #include <string>
47 #include <vector>
48 
49 #include <tf/transform_listener.h>
50 #include <geometry_msgs/PoseStamped.h>
51 #include <geometry_msgs/Twist.h>
52 
55 #include <mbf_utility/types.h>
56 
57 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
59 
61 {
62 
78  {
79  public:
80 
81  static const double DEFAULT_CONTROLLER_FREQUENCY;
82 
84 
92  const std::string name,
93  const mbf_abstract_core::AbstractController::Ptr& controller_ptr,
94  const ros::Publisher& vel_pub,
95  const ros::Publisher& goal_pub,
96  const TFPtr &tf_listener_ptr,
97  const MoveBaseFlexConfig &config);
98 
103 
108  virtual bool start();
109 
117  void setNewPlan(
118  const std::vector<geometry_msgs::PoseStamped> &plan,
119  bool tolerance_from_action = false,
120  double action_dist_tolerance = 1.0,
121  double action_angle_tolerance = 3.1415);
122 
128  virtual bool cancel();
129 
134  {
149  };
150 
156 
162 
169  geometry_msgs::TwistStamped getVelocityCmd();
170 
175  bool isPatienceExceeded();
176 
182  bool setControllerFrequency(double frequency);
183 
189  void reconfigure(const MoveBaseFlexConfig &config);
190 
195  bool isMoving();
196 
197  protected:
198 
209  virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped& pose,
210  const geometry_msgs::TwistStamped& velocity,
211  geometry_msgs::TwistStamped& vel_cmd, std::string& message);
212 
217  void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
218 
220  std::string plugin_name_;
221 
224 
227 
230 
233 
236 
239 
242 
246  virtual void run();
247 
255  virtual bool safetyCheck() { return true; };
256 
257  private:
258 
259 
263  void publishZeroVelocity();
264 
269  bool reachedGoalCheck();
270 
275  bool computeRobotPose();
276 
281  void setState(ControllerState state);
282 
284  boost::mutex state_mtx_;
285 
287  boost::mutex plan_mtx_;
288 
290  boost::mutex vel_cmd_mtx_;
291 
293  boost::mutex lct_mtx_;
294 
296  bool new_plan_;
297 
302  bool hasNewPlan();
303 
308  std::vector<geometry_msgs::PoseStamped> getNewPlan();
309 
311  geometry_msgs::TwistStamped vel_cmd_stamped_;
312 
314  std::vector<geometry_msgs::PoseStamped> plan_;
315 
317  boost::chrono::microseconds calling_duration_;
318 
320  std::string robot_frame_;
321 
323  std::string global_frame_;
324 
327 
330 
333 
335  double tf_timeout_;
336 
338  boost::mutex configuration_mutex_;
339 
341  bool moving_;
342 
345 
348 
351 
354 
357 
359  geometry_msgs::PoseStamped robot_pose_;
360 
363 
366 
369  };
370 
371 } /* namespace mbf_abstract_nav */
372 
373 #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
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.
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 planner execution. This calls the cancel method of the planner plugin. This could be usefu...
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.
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 Wed May 27 2020 04:03:14