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