41 #ifndef MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_ 42 #define MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_ 49 #include <geometry_msgs/PoseStamped.h> 89 const MoveBaseFlexConfig &config,
90 boost::function<
void()> setup_fn,
91 boost::function<
void()> cleanup_fn);
103 std::vector<geometry_msgs::PoseStamped>
getPlan();
163 void setNewGoal(
const geometry_msgs::PoseStamped &goal,
double tolerance);
177 void setNewStartAndGoal(
const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
187 bool start(
const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
195 void reconfigure(
const MoveBaseFlexConfig &config);
224 const geometry_msgs::PoseStamped &start,
225 const geometry_msgs::PoseStamped &goal,
227 std::vector<geometry_msgs::PoseStamped> &plan,
229 std::string &message);
262 std::vector<geometry_msgs::PoseStamped>
plan_;
ros::Time last_valid_plan_time_
the last time a valid plan has been computed.
PlanningState getState()
Returns the current internal state.
std::string plugin_name_
the name of the loaded planner plugin
geometry_msgs::PoseStamped goal_
the current goal pose used for planning
bool has_new_start_
true, if a new start pose has been set, until it is used.
double getFrequency()
Gets planning frequency.
int max_retries_
planning max retries
virtual uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance, std::vector< geometry_msgs::PoseStamped > &plan, double &cost, std::string &message)
calls the planner plugin to make a plan from the start pose to the goal pose with the given tolerance...
geometry_msgs::PoseStamped start_
the current start pose used for planning
The planner has been stopped.
double tolerance_
optional goal tolerance, in meters
bool isPatienceExceeded()
Checks whether the patience was exceeded.
No plan has been found (MAX_RETRIES and PAT_EXCEEDED are 0).
PlanningState state_
current internal state
void setState(PlanningState state)
Sets the internal state, thread communication safe.
boost::mutex configuration_mutex_
dynamic reconfigure mutex for a thread safe communication
The planner has been canceled.
AbstractPlannerExecution(const std::string name, const mbf_abstract_core::AbstractPlanner::Ptr planner_ptr, const MoveBaseFlexConfig &config, boost::function< void()> setup_fn, boost::function< void()> cleanup_fn)
Constructor.
boost::shared_ptr< AbstractPlannerExecution > Ptr
shared pointer type to the planner execution.
virtual void run()
The main run method, a thread will execute this method. It contains the main planner execution loop...
bool planning_
main cycle variable of the execution loop
double cost_
current global plan cost
Exceeded the maximum number of retries without a valid command.
ros::Time getLastValidPlanTime()
Returns the last time a valid plan was available.
ros::Time last_call_start_time_
the last call start time, updated each cycle.
An internal error occurred.
boost::mutex state_mtx_
mutex to handle safe thread communication for the current state
void setNewStart(const geometry_msgs::PoseStamped &start)
Sets a new start pose for the planner execution.
virtual bool cancel()
Cancel the planner execution. This calls the cancel method of the planner plugin. This could be usefu...
Exceeded the patience time without a valid command.
void setNewGoal(const geometry_msgs::PoseStamped &goal, double tolerance)
Sets a new goal pose for the planner execution.
double frequency_
planning cycle frequency (used only when running full navigation; we store here for grouping paramete...
std::vector< geometry_msgs::PoseStamped > getPlan()
Returns a new plan, if one is available.
boost::mutex plan_mtx_
mutex to handle safe thread communication for the plan and plan-costs
PlanningState
Internal states.
std::string global_frame_
the global frame in which the planner needs to plan
boost::mutex goal_start_mtx_
mutex to handle safe thread communication for the goal and start pose.
std::vector< geometry_msgs::PoseStamped > plan_
current global plan
void setNewStartAndGoal(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance)
Sets a new star and goal pose for the planner execution.
bool has_new_goal_
true, if a new goal pose has been set, until it is used.
mbf_abstract_core::AbstractPlanner::Ptr planner_
the local planer to calculate the velocity command
boost::mutex planning_mtx_
mutex to handle safe thread communication for the planning_ flag.
virtual ~AbstractPlannerExecution()
Destructor.
void reconfigure(const MoveBaseFlexConfig &config)
Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconf...
std::string robot_frame_
robot frame used for computing the current robot pose
const TFPtr tf_listener_ptr_
shared pointer to a common TransformListener
The AbstractPlannerExecution class loads and binds the global planner plugin. It contains a thread ru...
ros::Duration patience_
planning patience duration time
double getCost()
Gets computed costs.