#include <door_handler.h>
Public Member Functions | |
bool | checkClosePoses (const geometry_msgs::Pose &p1, const geometry_msgs::Pose &p2, float threshold=0.05, bool check_yaw=true) |
void | closeAllDoors () |
void | closeAllDoorsFarAwayFromPoint (const geometry_msgs::Pose &point, float distance=2.0) |
bool | closeDoor (const std::string &door) |
bool | closeDoor (int index) |
DoorHandler () | |
geometry_msgs::Pose | getDefaultLocation (bool is_door, int index) |
geometry_msgs::Pose | getDoorLocation (int index) |
float | getDoorWidth (int index) |
bool | isDoorOpen (const std::string &door) |
bool | isDoorOpen (int index) |
void | openAllDoors () |
bool | openDoor (const std::string &door) |
bool | openDoor (int index) |
void | spawnObject (bool is_door, int index=0) |
bool | teleportEntity (const std::string &entity, const geometry_msgs::Pose &pose) |
Private Member Functions | |
void | initialize () |
void | multimapHandler (const multi_level_map_msgs::MultiLevelMapData::ConstPtr &multimap) |
Private Attributes | |
std::vector< bool > | door_open_status_ |
std::vector< int > | door_to_true_door_map_ |
std::string | door_urdf_ |
std::vector < bwi_planning_common::Door > | doors_ |
ros::ServiceClient | get_gazebo_model_client_ |
bool | initialized_ |
ros::Subscriber | multimap_subscriber_ |
unsigned int | num_obstacles_ |
std::string | obstacle_urdf_ |
std::set< int > | obstacles_in_use |
ros::ServiceClient | set_gazebo_model_client_ |
ros::ServiceClient | spawn_model_client_ |
std::set< int > | unused_obstacles_ |
Definition at line 12 of file door_handler.h.
Definition at line 17 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::checkClosePoses | ( | const geometry_msgs::Pose & | p1, |
const geometry_msgs::Pose & | p2, | ||
float | threshold = 0.05 , |
||
bool | check_yaw = true |
||
) |
Definition at line 250 of file libsegbot_simulation_apps/door_handler.cpp.
Definition at line 209 of file libsegbot_simulation_apps/door_handler.cpp.
void segbot_simulation_apps::DoorHandler::closeAllDoorsFarAwayFromPoint | ( | const geometry_msgs::Pose & | point, |
float | distance = 2.0 |
||
) |
Definition at line 233 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::closeDoor | ( | const std::string & | door | ) |
Definition at line 184 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::closeDoor | ( | int | index | ) |
Definition at line 192 of file libsegbot_simulation_apps/door_handler.cpp.
geometry_msgs::Pose segbot_simulation_apps::DoorHandler::getDefaultLocation | ( | bool | is_door, |
int | index | ||
) |
Definition at line 118 of file libsegbot_simulation_apps/door_handler.cpp.
Definition at line 138 of file libsegbot_simulation_apps/door_handler.cpp.
float segbot_simulation_apps::DoorHandler::getDoorWidth | ( | int | index | ) |
Definition at line 134 of file libsegbot_simulation_apps/door_handler.cpp.
void segbot_simulation_apps::DoorHandler::initialize | ( | ) | [private] |
Definition at line 63 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::isDoorOpen | ( | const std::string & | door | ) |
Definition at line 217 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::isDoorOpen | ( | int | index | ) |
Definition at line 225 of file libsegbot_simulation_apps/door_handler.cpp.
void segbot_simulation_apps::DoorHandler::multimapHandler | ( | const multi_level_map_msgs::MultiLevelMapData::ConstPtr & | multimap | ) | [private] |
Definition at line 101 of file libsegbot_simulation_apps/door_handler.cpp.
Definition at line 178 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::openDoor | ( | const std::string & | door | ) |
Definition at line 154 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::openDoor | ( | int | index | ) |
Definition at line 162 of file libsegbot_simulation_apps/door_handler.cpp.
void segbot_simulation_apps::DoorHandler::spawnObject | ( | bool | is_door, |
int | index = 0 |
||
) |
Definition at line 297 of file libsegbot_simulation_apps/door_handler.cpp.
bool segbot_simulation_apps::DoorHandler::teleportEntity | ( | const std::string & | entity, |
const geometry_msgs::Pose & | pose | ||
) |
Definition at line 266 of file libsegbot_simulation_apps/door_handler.cpp.
std::vector<bool> segbot_simulation_apps::DoorHandler::door_open_status_ [private] |
Definition at line 50 of file door_handler.h.
std::vector<int> segbot_simulation_apps::DoorHandler::door_to_true_door_map_ [private] |
Definition at line 51 of file door_handler.h.
std::string segbot_simulation_apps::DoorHandler::door_urdf_ [private] |
Definition at line 62 of file door_handler.h.
std::vector<bwi_planning_common::Door> segbot_simulation_apps::DoorHandler::doors_ [private] |
Definition at line 49 of file door_handler.h.
Definition at line 57 of file door_handler.h.
bool segbot_simulation_apps::DoorHandler::initialized_ [private] |
Definition at line 43 of file door_handler.h.
Definition at line 46 of file door_handler.h.
unsigned int segbot_simulation_apps::DoorHandler::num_obstacles_ [private] |
Definition at line 55 of file door_handler.h.
std::string segbot_simulation_apps::DoorHandler::obstacle_urdf_ [private] |
Definition at line 61 of file door_handler.h.
std::set<int> segbot_simulation_apps::DoorHandler::obstacles_in_use [private] |
Definition at line 53 of file door_handler.h.
Definition at line 58 of file door_handler.h.
Definition at line 59 of file door_handler.h.
std::set<int> segbot_simulation_apps::DoorHandler::unused_obstacles_ [private] |
Definition at line 54 of file door_handler.h.