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 <geometry_msgs/Pose.h> 00006 #include <ros/ros.h> 00007 00008 #include <bwi_planning_common/structures.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 std::vector<bwi_planning_common::Door> doors_; 00044 std::vector<std::string> locations_; 00045 std::vector<int32_t> location_map_; 00046 00047 std::vector<bool> door_open_status_; 00048 std::set<int> obstacles_in_use; 00049 std::set<int> unused_obstacles_; 00050 unsigned int num_obstacles_; // obstacles + doors 00051 00052 ros::ServiceClient get_gazebo_model_client_; 00053 ros::ServiceClient set_gazebo_model_client_; 00054 ros::ServiceClient spawn_model_client_; 00055 00056 std::string obstacle_urdf_; 00057 std::string door_urdf_; 00058 }; 00059 } 00060 00061 #endif /* end of include guard: SEGBOT_SIM_APPS_DOOR_HANDLER_H */