AutoCar  v1.0.0
move_base.h
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef NAV_MOVE_BASE_ACTION_H_
38 #define NAV_MOVE_BASE_ACTION_H_
39 
40 #include <vector>
41 #include <string>
42 
43 #include <ros/ros.h>
44 
45 #include <actionlib/server/simple_action_server.h>
46 #include <move_base_msgs/MoveBaseAction.h>
47 
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>
55 
56 #include <pluginlib/class_loader.h>
57 #include <std_srvs/Empty.h>
58 
59 #include <dynamic_reconfigure/server.h>
60 #include "move_base/MoveBaseConfig.h"
61 
62 namespace move_base {
63  //typedefs to help us out with the action server so that we don't hace to type so much
64  typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
65 
66  enum MoveBaseState {
67  PLANNING,
68  CONTROLLING,
69  CLEARING
70  };
71 
72  enum RecoveryTrigger
73  {
74  PLANNING_R,
75  CONTROLLING_R,
76  OSCILLATION_R
77  };
78 
79  enum Mode
80  {
81  PATROLLING,
82  SHOOTING,
83  POINT
84  };
85 
90  class MoveBase {
91  public:
97  MoveBase(tf::TransformListener& tf);
98 
102  virtual ~MoveBase();
103 
110  bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);
111 
112  private:
119  bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
120 
127  bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
128 
135  bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
136 
142  bool loadRecoveryBehaviors(ros::NodeHandle node);
143 
147  void loadDefaultRecoveryBehaviors();
148 
154  void clearCostmapWindows(double size_x, double size_y);
155 
159  void publishZeroVelocity();
160 
164  void resetState();
165 
166  void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
167 
168  void planThread();
169 
170  void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
171 
172  bool isQuaternionValid(const geometry_msgs::Quaternion& q);
173 
174  double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
175 
176  geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
177 
181  void wakePlanner(const ros::TimerEvent& event);
182 
183  Mode modeDetector(const geometry_msgs::PoseStamped& goal_pose_msg);
184 
185  Mode mode_;
186 
187  tf::TransformListener& tf_;
188 
189  MoveBaseActionServer* as_;
190 
191  boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
192  costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
193 
194  boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
195  std::string robot_base_frame_, global_frame_;
196 
197  std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;
198  unsigned int recovery_index_;
199 
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_;
209 
210  MoveBaseState state_;
211  RecoveryTrigger recovery_trigger_;
212 
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_;
218 
219  //set up plan triple buffer
220  std::vector<geometry_msgs::PoseStamped>* planner_plan_;
221  std::vector<geometry_msgs::PoseStamped>* latest_plan_;
222  std::vector<geometry_msgs::PoseStamped>* controller_plan_;
223 
224  //set up the planner's thread
225  bool runPlanner_;
226  boost::mutex planner_mutex_;
227  boost::condition_variable planner_cond_;
228  geometry_msgs::PoseStamped planner_goal_;
229  boost::thread* planner_thread_;
230 
231 
232  boost::recursive_mutex configuration_mutex_;
233  dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
234 
235  void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
236 
237  move_base::MoveBaseConfig last_config_;
238  move_base::MoveBaseConfig default_config_;
239  bool setup_, p_freq_change_, c_freq_change_;
240  bool new_global_plan_;
241  };
242 };
243 #endif
244 
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.