37 #ifndef NAV_MOVE_BASE_ACTION_H_ 38 #define NAV_MOVE_BASE_ACTION_H_ 45 #include <actionlib/server/simple_action_server.h> 46 #include <move_base_msgs/MoveBaseAction.h> 48 #include <nav_core/base_local_planner.h> 49 #include <nav_core/base_global_planner.h> 50 #include <nav_core/recovery_behavior.h> 51 #include <geometry_msgs/PoseStamped.h> 52 #include <costmap_2d/costmap_2d_ros.h> 53 #include <costmap_2d/costmap_2d.h> 54 #include <nav_msgs/GetPlan.h> 56 #include <pluginlib/class_loader.h> 57 #include <std_srvs/Empty.h> 59 #include <dynamic_reconfigure/server.h> 60 #include "move_base/MoveBaseConfig.h" 64 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
110 bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);
119 bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
127 bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
135 bool makePlan(
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
142 bool loadRecoveryBehaviors(ros::NodeHandle node);
147 void loadDefaultRecoveryBehaviors();
154 void clearCostmapWindows(
double size_x,
double size_y);
159 void publishZeroVelocity();
166 void goalCB(
const geometry_msgs::PoseStamped::ConstPtr& goal);
170 void executeCb(
const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
172 bool isQuaternionValid(
const geometry_msgs::Quaternion& q);
174 double distance(
const geometry_msgs::PoseStamped& p1,
const geometry_msgs::PoseStamped& p2);
176 geometry_msgs::PoseStamped goalToGlobalFrame(
const geometry_msgs::PoseStamped& goal_pose_msg);
181 void wakePlanner(
const ros::TimerEvent& event);
183 Mode modeDetector(
const geometry_msgs::PoseStamped& goal_pose_msg);
187 tf::TransformListener& tf_;
189 MoveBaseActionServer* as_;
191 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
192 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
194 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
195 std::string robot_base_frame_, global_frame_;
197 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;
198 unsigned int recovery_index_;
200 tf::Stamped<tf::Pose> global_pose_;
201 double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
202 double planner_patience_, controller_patience_;
203 double conservative_reset_dist_, clearing_radius_;
204 ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_;
205 ros::Subscriber goal_sub_;
206 ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
207 bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
208 double oscillation_timeout_, oscillation_distance_;
210 MoveBaseState state_;
211 RecoveryTrigger recovery_trigger_;
213 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
214 geometry_msgs::PoseStamped oscillation_pose_;
215 pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
216 pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
217 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
220 std::vector<geometry_msgs::PoseStamped>* planner_plan_;
221 std::vector<geometry_msgs::PoseStamped>* latest_plan_;
222 std::vector<geometry_msgs::PoseStamped>* controller_plan_;
226 boost::mutex planner_mutex_;
227 boost::condition_variable planner_cond_;
228 geometry_msgs::PoseStamped planner_goal_;
229 boost::thread* planner_thread_;
232 boost::recursive_mutex configuration_mutex_;
233 dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
239 bool setup_, p_freq_change_, c_freq_change_;
240 bool new_global_plan_;
virtual ~MoveBase()
Destructor - Cleans up.
A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location...
Definition: move_base.h:90
Definition: MoveBaseConfig.h:26
Definition: MoveBaseConfig.h:22
MoveBase(tf::TransformListener &tf)
Constructor for the actions.
bool executeCycle(geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &global_plan)
Performs a control cycle.