Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef NAV_MOVE_BASE_ACTION_H_
00038 #define NAV_MOVE_BASE_ACTION_H_
00039 #include <cmath>
00040 #include <ros/ros.h>
00041
00042 #include <actionlib/server/simple_action_server.h>
00043 #include <move_base_msgs/MoveBaseAction.h>
00044
00045 #include <nav_core/base_local_planner.h>
00046 #include <nav_core/base_global_planner.h>
00047 #include <nav_core/recovery_behavior.h>
00048 #include <geometry_msgs/PoseStamped.h>
00049 #include <costmap_2d/costmap_2d_ros.h>
00050 #include <costmap_2d/costmap_2d.h>
00051 #include <vector>
00052 #include <string>
00053 #include <nav_msgs/GetPlan.h>
00054 #include <geometry_msgs/Twist.h>
00055
00056 #include <pluginlib/class_loader.h>
00057 #include <std_srvs/Empty.h>
00058
00059 #include <dynamic_reconfigure/server.h>
00060 #include "move_base/MoveBaseConfig.h"
00061
00062 namespace move_base {
00063
00064 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
00065
00066 enum MoveBaseState {
00067 PLANNING,
00068 CONTROLLING,
00069 CLEARING
00070 };
00071
00072 enum RecoveryTrigger
00073 {
00074 PLANNING_R,
00075 CONTROLLING_R,
00076 OSCILLATION_R
00077 };
00078
00083 class MoveBase {
00084 public:
00090 MoveBase(std::string name, tf::TransformListener& tf);
00091
00095 virtual ~MoveBase();
00096
00103 bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);
00104
00105 private:
00112 bool clearUnknownService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
00113
00114 private:
00121 bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
00122
00129 bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
00130
00137 bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
00138
00144 bool loadRecoveryBehaviors(ros::NodeHandle node);
00145
00149 void loadDefaultRecoveryBehaviors();
00150
00156 void clearCostmapWindows(double size_x, double size_y);
00157
00161 void publishZeroVelocity();
00162
00166 void resetState();
00167
00168 void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
00169
00170 void planThread();
00171
00172 void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
00173
00174 bool isQuaternionValid(const geometry_msgs::Quaternion& q);
00175
00176 double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
00177
00178 geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
00179
00180 tf::TransformListener& tf_;
00181
00182 MoveBaseActionServer* as_;
00183
00184 nav_core::BaseLocalPlanner* tc_;
00185 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
00186
00187 nav_core::BaseGlobalPlanner* planner_;
00188 std::string robot_base_frame_, global_frame_;
00189
00190 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;
00191 unsigned int recovery_index_;
00192
00193 tf::Stamped<tf::Pose> global_pose_;
00194 double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
00195 double planner_patience_, controller_patience_;
00196 double conservative_reset_dist_, clearing_radius_;
00197 ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_;
00198 ros::Subscriber goal_sub_;
00199 ros::ServiceServer make_plan_srv_, clear_unknown_srv_, clear_costmaps_srv_;
00200 bool shutdown_costmaps_, clearing_roatation_allowed_, recovery_behavior_enabled_;
00201 double oscillation_timeout_, oscillation_distance_;
00202
00203 MoveBaseState state_;
00204 RecoveryTrigger recovery_trigger_;
00205
00206 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
00207 geometry_msgs::PoseStamped oscillation_pose_;
00208 pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
00209 pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
00210 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
00211
00212
00213 std::vector<geometry_msgs::PoseStamped>* planner_plan_;
00214 std::vector<geometry_msgs::PoseStamped>* latest_plan_;
00215 std::vector<geometry_msgs::PoseStamped>* controller_plan_;
00216
00217
00218 bool runPlanner_;
00219 boost::mutex planner_mutex_;
00220 boost::condition_variable planner_cond_;
00221 geometry_msgs::PoseStamped planner_goal_;
00222 boost::thread* planner_thread_;
00223
00224
00225 boost::recursive_mutex configuration_mutex_;
00226 dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
00227
00228 void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
00229
00230 move_base::MoveBaseConfig last_config_;
00231 move_base::MoveBaseConfig default_config_;
00232 bool setup_, p_freq_change_, c_freq_change_;
00233 bool new_global_plan_;
00234 };
00235 };
00236 #endif
00237