00001 #ifndef FORCE_ROTATE_RECOVERY_H 00002 #define FORCE_ROTATE_RECOVERY_H 00003 00004 #include <nav_core/recovery_behavior.h> 00005 #include <costmap_2d/costmap_2d_ros.h> 00006 #include <tf/transform_listener.h> 00007 #include <ros/ros.h> 00008 #include <base_local_planner/costmap_model.h> 00009 #include <geometry_msgs/Twist.h> 00010 #include <geometry_msgs/Point.h> 00011 #include <angles/angles.h> 00012 00013 namespace force_rotate_recovery{ 00014 class ForceRotateRecovery : public nav_core::RecoveryBehavior { 00015 public: 00016 ForceRotateRecovery(); 00017 ~ForceRotateRecovery(){ delete world_model_; } 00018 void initialize(std::string name, tf::TransformListener* tf, 00019 costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap); 00020 00021 void runBehavior(); 00022 00023 private: 00024 costmap_2d::Costmap2DROS* global_costmap_, *local_costmap_; 00025 costmap_2d::Costmap2D costmap_; 00026 std::string name_; 00027 bool initialized_; 00028 double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_; 00029 base_local_planner::CostmapModel* world_model_; 00030 }; 00031 }; 00032 00033 #endif //FORCE_ROTATE_RECOVERY_H