segbot_logical_translator.h
Go to the documentation of this file.
00001 
00039 #ifndef SEGBOT_LOGICAL_TRANSLATOR_G54K6X7H
00040 #define SEGBOT_LOGICAL_TRANSLATOR_G54K6X7H
00041 
00042 #include <ros/ros.h>
00043 #include <boost/shared_ptr.hpp>
00044 
00045 #include <bwi_mapper/path_finder.h>
00046 #include <bwi_planning_common/structures.h>
00047 #include <bwi_planning_common/utils.h>
00048 #include <bwi_tools/point.h>
00049 #include <nav_msgs/GetPlan.h>
00050 
00051 namespace segbot_logical_translator {
00052 
00053   class SegbotLogicalTranslator {
00054 
00055     public:
00056 
00057       SegbotLogicalTranslator();
00058 
00059       bool isDoorOpen(size_t idx);
00060 
00061       bool getApproachPoint(size_t idx, 
00062           const bwi::Point2f& current_location,
00063           bwi::Point2f& point, float &yaw);
00064 
00065       bool getThroughDoorPoint(size_t idx, 
00066           const bwi::Point2f& current_location,
00067           bwi::Point2f& point, float& yaw);
00068 
00069       bool isRobotFacingDoor(
00070           const bwi::Point2f& current_location,
00071           float yaw, float threshold, size_t idx);
00072       bool isRobotBesideDoor(
00073           const bwi::Point2f& current_location,
00074           float yaw, float threshold, size_t idx);
00075       bool initialize();
00076 
00077       bool isObjectApproachable(const std::string& object_name, 
00078           const bwi::Point2f& current_location);
00079 
00080       inline bool getObjectApproachLocation(const std::string& object_name,
00081           geometry_msgs::Pose& pose) {
00082         if (object_approach_map_.find(object_name) ==
00083             object_approach_map_.end()) {
00084           return false; 
00085         }
00086         pose = object_approach_map_[object_name];
00087         return true;
00088       }
00089 
00090       size_t getLocationIdx(
00091           const bwi::Point2f& current_location);
00092 
00093       inline size_t getLocationIdx(
00094           const std::string& loc_str) const {
00095         for (size_t i = 0; i < locations_.size(); ++i) {
00096           if (locations_[i] == loc_str) {
00097             return i;
00098           }
00099         }
00100         return (size_t)-1;
00101       }
00102 
00103       inline size_t getDoorIdx(const std::string& door_str) const {
00104         for (size_t i = 0; i < doors_.size(); ++i) {
00105           if (doors_[i].name == door_str) {
00106             return i;
00107           }
00108         }
00109         return (size_t)-1;
00110       }
00111 
00112       inline std::string getLocationString(size_t idx) const {
00113         if (idx >= locations_.size())
00114           return "";
00115         return locations_[idx];
00116       }
00117 
00118       inline std::string getDoorString(size_t idx) const {
00119         if (idx >= doors_.size())
00120           return "";
00121         return doors_[idx].name;
00122       }
00123 
00124       inline size_t getNumDoors() const {
00125         return doors_.size();
00126       }
00127 
00128     protected:
00129 
00130       std::string global_frame_id_;
00131 
00132       std::vector<bwi_planning_common::Door> doors_;
00133       std::map<int, boost::shared_ptr<bwi_mapper::PathFinder> > door_approachable_space_1_; 
00134       std::map<int, boost::shared_ptr<bwi_mapper::PathFinder> > door_approachable_space_2_; 
00135 
00136       std::vector<std::string> locations_;
00137       std::vector<int32_t> location_map_;
00138       std::map<std::string, geometry_msgs::Pose> object_approach_map_;
00139       std::map<std::string, boost::shared_ptr<bwi_mapper::PathFinder> > object_approachable_space_;
00140 
00141       nav_msgs::OccupancyGrid map_;
00142       nav_msgs::OccupancyGrid map_with_doors_;
00143       nav_msgs::OccupancyGrid inflated_map_with_doors_;
00144       nav_msgs::MapMetaData info_;
00145 
00146       boost::shared_ptr<ros::NodeHandle> nh_;
00147       ros::ServiceClient make_plan_client_;
00148       bool make_plan_client_initialized_;
00149 
00150       bool initialized_;
00151 
00152   }; /* SegbotLogicalTranslator */
00153   
00154 } /* segbot_logical_translator */
00155 
00156 #endif /* end of include guard: SEGBOT_LOGICAL_TRANSLATOR_G54K6X7H */


segbot_logical_translator
Author(s): Piyush Khandelwal
autogenerated on Thu Aug 27 2015 15:11:22