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 
129  virtual bool cancel();
130 
135  {
150  };
151 
156  ControllerState getState() const;
157 
163 
170  geometry_msgs::TwistStamped getVelocityCmd() const;
171 
176  bool isPatienceExceeded() const;
177 
183  bool setControllerFrequency(double frequency);
184 
190  void reconfigure(const MoveBaseFlexConfig &config);
191 
196  bool isMoving() const;
197 
198  protected:
199 
210  virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped &pose,
211  const geometry_msgs::TwistStamped &velocity,
212  geometry_msgs::TwistStamped &vel_cmd, std::string &message);
213 
218  void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
219 
221  std::string plugin_name_;
222 
225 
228 
231 
234 
237 
240 
243 
245  std::string robot_frame_;
246 
248  std::string global_frame_;
249 
253  virtual void run();
254 
262  virtual bool safetyCheck() { return true; };
263 
264  private:
265 
266 
270  void publishZeroVelocity();
271 
276  bool reachedGoalCheck();
277 
282  bool computeRobotPose();
283 
288  void setState(ControllerState state);
289 
291  mutable boost::mutex state_mtx_;
292 
294  mutable boost::mutex plan_mtx_;
295 
297  mutable boost::mutex vel_cmd_mtx_;
298 
300  mutable boost::mutex lct_mtx_;
301 
303  bool new_plan_;
304 
309  bool hasNewPlan();
310 
315  std::vector<geometry_msgs::PoseStamped> getNewPlan();
316 
318  geometry_msgs::TwistStamped vel_cmd_stamped_;
319 
321  std::vector<geometry_msgs::PoseStamped> plan_;
322 
325 
328 
331 
334 
336  double tf_timeout_;
337 
339  boost::mutex configuration_mutex_;
340 
342  bool moving_;
343 
346 
349 
352 
355 
358 
360  geometry_msgs::PoseStamped robot_pose_;
361 
364 
367 
370  };
371 
372 } /* namespace mbf_abstract_nav */
373 
374 #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.
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.
ros::Rate loop_rate_
the loop_rate which corresponds with the controller frequency.
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. Normally called upon aborting the navigation. This calls the cancel ...
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() const
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 isPatienceExceeded() const
Checks whether the patience duration time has been exceeded, ot not.
bool moving_
main controller loop variable, true if the controller is running, false otherwise ...
geometry_msgs::TwistStamped getVelocityCmd() const
Returns the last velocity command calculated by the plugin. Set by setVelocityCmd method...
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.
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 ...
ControllerState getState() const
Return the current state of the controller execution. Thread communication safe.
boost::mutex state_mtx_
mutex to handle safe thread communication for the current value of the state
Base class for running concurrent navigation tasks.
bool force_stop_on_cancel_
whether move base flex should force the robot to stop on canceling navigation.
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.
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.
ros::Time getLastPluginCallTime() const
Returns the time of the last plugin call.
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
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 Mon Feb 28 2022 22:49:50