00001 #ifndef SEGBOT_SIM_APPS_DOOR_HANDLER_H 00002 #define SEGBOT_SIM_APPS_DOOR_HANDLER_H 00003 00004 #include <boost/shared_ptr.hpp> 00005 #include <ros/ros.h> 00006 00007 #include <bwi_planning_common/structures.h> 00008 #include <multi_level_map_msgs/MultiLevelMapData.h> 00009 00010 #include <segbot_simulation_apps/common.h> 00011 00012 namespace segbot_simulation_apps { 00013 00014 class DoorHandler { 00015 00016 public: 00017 00018 DoorHandler (); 00019 00020 geometry_msgs::Pose getDefaultLocation(bool is_door, int index); 00021 float getDoorWidth(int index); 00022 geometry_msgs::Pose getDoorLocation(int index); 00023 00024 bool openDoor(const std::string& door); 00025 bool openDoor(int index); 00026 void openAllDoors(); 00027 bool closeDoor(const std::string& door); 00028 bool closeDoor(int index); 00029 void closeAllDoors(); 00030 bool isDoorOpen(const std::string& door); 00031 bool isDoorOpen(int index); 00032 00033 void closeAllDoorsFarAwayFromPoint( 00034 const geometry_msgs::Pose& point, float distance = 2.0); 00035 00036 void spawnObject(bool is_door, int index = 0); 00037 00038 private: 00039 00040 bool initialized_; 00041 void initialize(); 00042 00043 ros::Subscriber multimap_subscriber_; 00044 void multimapHandler(const multi_level_map_msgs::MultiLevelMapData::ConstPtr& multimap); 00045 00046 std::vector<bwi_planning_common::Door> doors_; 00047 std::vector<bool> door_open_status_; 00048 std::vector<int> door_to_true_door_map_; 00049 00050 std::set<int> obstacles_in_use; 00051 std::set<int> unused_obstacles_; 00052 unsigned int num_obstacles_; // obstacles + doors 00053 00054 ros::ServiceClient get_gazebo_model_client_; 00055 ros::ServiceClient set_gazebo_model_client_; 00056 ros::ServiceClient spawn_model_client_; 00057 00058 std::string obstacle_urdf_; 00059 std::string door_urdf_; 00060 }; 00061 } 00062 00063 #endif /* end of include guard: SEGBOT_SIM_APPS_DOOR_HANDLER_H */