segbot_logical_translator.h

Go to the documentation of this file.

#ifndef SEGBOT_LOGICAL_TRANSLATOR_G54K6X7H
#define SEGBOT_LOGICAL_TRANSLATOR_G54K6X7H

#include <ros/ros.h>
#include <boost/shared_ptr.hpp>

#include <bwi_mapper/path_finder.h>
#include <bwi_planning_common/structures.h>
#include <bwi_planning_common/utils.h>
#include <bwi_tools/point.h>
#include <nav_msgs/GetPlan.h>

namespace segbot_logical_translator {

  class SegbotLogicalTranslator {

    public:

      SegbotLogicalTranslator();

      bool isDoorOpen(size_t idx);

      bool getApproachPoint(size_t idx, 
          const bwi::Point2f& current_location,
          bwi::Point2f& point, float &yaw);

      bool getThroughDoorPoint(size_t idx, 
          const bwi::Point2f& current_location,
          bwi::Point2f& point, float& yaw);

      bool isRobotFacingDoor(
          const bwi::Point2f& current_location,
          float yaw, float threshold, size_t idx);
      bool isRobotBesideDoor(
          const bwi::Point2f& current_location,
          float yaw, float threshold, size_t idx);
      bool initialize();

      bool isObjectApproachable(const std::string& object_name, 
          const bwi::Point2f& current_location);

      inline bool getObjectApproachLocation(const std::string& object_name,
          geometry_msgs::Pose& pose) {
        if (object_approach_map_.find(object_name) ==
            object_approach_map_.end()) {
          return false; 
        }
        pose = object_approach_map_[object_name];
        return true;
      }

      size_t getLocationIdx(
          const bwi::Point2f& current_location);

      inline size_t getLocationIdx(
          const std::string& loc_str) const {
        for (size_t i = 0; i < locations_.size(); ++i) {
          if (locations_[i] == loc_str) {
            return i;
          }
        }
        return (size_t)-1;
      }

      inline size_t getDoorIdx(const std::string& door_str) const {
        for (size_t i = 0; i < doors_.size(); ++i) {
          if (doors_[i].name == door_str) {
            return i;
          }
        }
        return (size_t)-1;
      }

      inline std::string getLocationString(size_t idx) const {
        if (idx >= locations_.size())
          return "";
        return locations_[idx];
      }

      inline std::string getDoorString(size_t idx) const {
        if (idx >= doors_.size())
          return "";
        return doors_[idx].name;
      }

      inline size_t getNumDoors() const {
        return doors_.size();
      }

    protected:

      std::string global_frame_id_;

      std::vector<bwi_planning_common::Door> doors_;
      std::map<int, boost::shared_ptr<bwi_mapper::PathFinder> > door_approachable_space_1_; 
      std::map<int, boost::shared_ptr<bwi_mapper::PathFinder> > door_approachable_space_2_; 

      std::vector<std::string> locations_;
      std::vector<int32_t> location_map_;
      std::map<std::string, geometry_msgs::Pose> object_approach_map_;
      std::map<std::string, boost::shared_ptr<bwi_mapper::PathFinder> > object_approachable_space_;

      nav_msgs::OccupancyGrid map_;
      nav_msgs::OccupancyGrid map_with_doors_;
      nav_msgs::OccupancyGrid inflated_map_with_doors_;
      nav_msgs::MapMetaData info_;

      boost::shared_ptr<ros::NodeHandle> nh_;
      ros::ServiceClient make_plan_client_;
      ros::ServiceClient static_costmap_toggle_client_;
      bool make_plan_client_initialized_;

      bool static_costmap_toggle_client_initialized_;
      void initializeStaticCostmapToggleService();
      void enableStaticCostmap(bool value);

      bool initialized_;

  }; /* SegbotLogicalTranslator */
  
} /* segbot_logical_translator */

#endif /* end of include guard: SEGBOT_LOGICAL_TRANSLATOR_G54K6X7H */