00001 #pragma once 00002 00003 #include "straf_recovery/StrafRecoveryConfig.h" 00004 00005 #include <base_local_planner/costmap_model.h> 00006 #include <costmap_2d/costmap_2d_ros.h> 00007 #include <nav_core/recovery_behavior.h> 00008 #include <geometry_msgs/PoseStamped.h> 00009 #include <ros/ros.h> 00010 00011 namespace straf_recovery 00012 { 00013 class StrafRecovery : public nav_core::RecoveryBehavior 00014 { 00015 public: 00016 StrafRecovery(); 00017 void initialize(std::string, tf::TransformListener* tf, costmap_2d::Costmap2DROS* global_costmap, 00018 costmap_2d::Costmap2DROS* local_costmap); 00019 00028 void runBehavior(); 00029 00030 private: 00031 bool initialized_; 00032 bool enabled_; 00033 double frequency_; 00034 double maximum_translate_distance_; 00035 double minimum_translate_distance_; 00036 double go_to_goal_distance_threshold_; 00037 double xy_goal_tolerance_; 00038 double vel_; 00039 int timeout_; // in seconds 00040 int cycles_; // track how many times we've run for detecting failure? 00041 base_local_planner::CostmapModel* local_costmap_model_; 00042 dynamic_reconfigure::Server<StrafRecoveryConfig>* dsrv_; 00043 costmap_2d::Costmap2DROS* local_costmap_; 00044 costmap_2d::Costmap2DROS* global_costmap_; 00045 geometry_msgs::PoseStamped last_goal_; 00046 ros::Publisher cycles_pub_; 00047 ros::Publisher obstacle_pub_; 00048 ros::Publisher vel_pub_; 00049 ros::Subscriber goal_sub_; 00050 std::string name_; 00051 tf::TransformListener* tf_; 00052 00054 void strafInDiretionOfPose(tf::Stamped<tf::Pose> current_pose, tf::Vector3 direction_pose, bool away=true); 00055 00057 void goalCallback(const geometry_msgs::PoseStamped& msg); 00058 00060 void reconfigureCB(StrafRecoveryConfig& config, uint32_t level); 00061 00062 }; 00063 }