#include <segbot_logical_translator.h>
Public Member Functions | |
bool | getApproachPoint (size_t idx, const bwi::Point2f ¤t_location, bwi::Point2f &point, float &yaw) |
size_t | getDoorIdx (const std::string &door_str) const |
std::string | getDoorString (size_t idx) const |
size_t | getLocationIdx (const bwi::Point2f ¤t_location) |
size_t | getLocationIdx (const std::string &loc_str) const |
std::string | getLocationString (size_t idx) const |
size_t | getNumDoors () const |
bool | getObjectApproachLocation (const std::string &object_name, geometry_msgs::Pose &pose) |
bool | getThroughDoorPoint (size_t idx, const bwi::Point2f ¤t_location, bwi::Point2f &point, float &yaw) |
bool | initialize () |
bool | isDoorOpen (size_t idx) |
bool | isObjectApproachable (const std::string &object_name, const bwi::Point2f ¤t_location) |
bool | isRobotBesideDoor (const bwi::Point2f ¤t_location, float yaw, float threshold, size_t idx) |
bool | isRobotFacingDoor (const bwi::Point2f ¤t_location, float yaw, float threshold, size_t idx) |
SegbotLogicalTranslator () | |
Protected Attributes | |
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 < bwi_planning_common::Door > | doors_ |
std::string | global_frame_id_ |
nav_msgs::OccupancyGrid | inflated_map_with_doors_ |
nav_msgs::MapMetaData | info_ |
bool | initialized_ |
std::vector< int32_t > | location_map_ |
std::vector< std::string > | locations_ |
ros::ServiceClient | make_plan_client_ |
bool | make_plan_client_initialized_ |
nav_msgs::OccupancyGrid | map_ |
nav_msgs::OccupancyGrid | map_with_doors_ |
boost::shared_ptr < ros::NodeHandle > | nh_ |
std::map< std::string, geometry_msgs::Pose > | object_approach_map_ |
std::map< std::string, boost::shared_ptr < bwi_mapper::PathFinder > > | object_approachable_space_ |
Definition at line 53 of file segbot_logical_translator.h.
Definition at line 53 of file segbot_logical_translator.cpp.
bool segbot_logical_translator::SegbotLogicalTranslator::getApproachPoint | ( | size_t | idx, |
const bwi::Point2f & | current_location, | ||
bwi::Point2f & | point, | ||
float & | yaw | ||
) |
Definition at line 202 of file segbot_logical_translator.cpp.
size_t segbot_logical_translator::SegbotLogicalTranslator::getDoorIdx | ( | const std::string & | door_str | ) | const [inline] |
Definition at line 103 of file segbot_logical_translator.h.
std::string segbot_logical_translator::SegbotLogicalTranslator::getDoorString | ( | size_t | idx | ) | const [inline] |
Definition at line 118 of file segbot_logical_translator.h.
size_t segbot_logical_translator::SegbotLogicalTranslator::getLocationIdx | ( | const bwi::Point2f & | current_location | ) |
Definition at line 343 of file segbot_logical_translator.cpp.
size_t segbot_logical_translator::SegbotLogicalTranslator::getLocationIdx | ( | const std::string & | loc_str | ) | const [inline] |
Definition at line 93 of file segbot_logical_translator.h.
std::string segbot_logical_translator::SegbotLogicalTranslator::getLocationString | ( | size_t | idx | ) | const [inline] |
Definition at line 112 of file segbot_logical_translator.h.
size_t segbot_logical_translator::SegbotLogicalTranslator::getNumDoors | ( | ) | const [inline] |
Definition at line 124 of file segbot_logical_translator.h.
bool segbot_logical_translator::SegbotLogicalTranslator::getObjectApproachLocation | ( | const std::string & | object_name, |
geometry_msgs::Pose & | pose | ||
) | [inline] |
Definition at line 80 of file segbot_logical_translator.h.
bool segbot_logical_translator::SegbotLogicalTranslator::getThroughDoorPoint | ( | size_t | idx, |
const bwi::Point2f & | current_location, | ||
bwi::Point2f & | point, | ||
float & | yaw | ||
) |
Definition at line 267 of file segbot_logical_translator.cpp.
Definition at line 58 of file segbot_logical_translator.cpp.
bool segbot_logical_translator::SegbotLogicalTranslator::isDoorOpen | ( | size_t | idx | ) |
Definition at line 116 of file segbot_logical_translator.cpp.
bool segbot_logical_translator::SegbotLogicalTranslator::isObjectApproachable | ( | const std::string & | object_name, |
const bwi::Point2f & | current_location | ||
) |
Definition at line 247 of file segbot_logical_translator.cpp.
bool segbot_logical_translator::SegbotLogicalTranslator::isRobotBesideDoor | ( | const bwi::Point2f & | current_location, |
float | yaw, | ||
float | threshold, | ||
size_t | idx | ||
) |
Definition at line 325 of file segbot_logical_translator.cpp.
bool segbot_logical_translator::SegbotLogicalTranslator::isRobotFacingDoor | ( | const bwi::Point2f & | current_location, |
float | yaw, | ||
float | threshold, | ||
size_t | idx | ||
) |
Definition at line 299 of file segbot_logical_translator.cpp.
std::map<int, boost::shared_ptr<bwi_mapper::PathFinder> > segbot_logical_translator::SegbotLogicalTranslator::door_approachable_space_1_ [protected] |
Definition at line 133 of file segbot_logical_translator.h.
std::map<int, boost::shared_ptr<bwi_mapper::PathFinder> > segbot_logical_translator::SegbotLogicalTranslator::door_approachable_space_2_ [protected] |
Definition at line 134 of file segbot_logical_translator.h.
std::vector<bwi_planning_common::Door> segbot_logical_translator::SegbotLogicalTranslator::doors_ [protected] |
Definition at line 132 of file segbot_logical_translator.h.
std::string segbot_logical_translator::SegbotLogicalTranslator::global_frame_id_ [protected] |
Definition at line 130 of file segbot_logical_translator.h.
nav_msgs::OccupancyGrid segbot_logical_translator::SegbotLogicalTranslator::inflated_map_with_doors_ [protected] |
Definition at line 143 of file segbot_logical_translator.h.
nav_msgs::MapMetaData segbot_logical_translator::SegbotLogicalTranslator::info_ [protected] |
Definition at line 144 of file segbot_logical_translator.h.
bool segbot_logical_translator::SegbotLogicalTranslator::initialized_ [protected] |
Definition at line 150 of file segbot_logical_translator.h.
std::vector<int32_t> segbot_logical_translator::SegbotLogicalTranslator::location_map_ [protected] |
Definition at line 137 of file segbot_logical_translator.h.
std::vector<std::string> segbot_logical_translator::SegbotLogicalTranslator::locations_ [protected] |
Definition at line 136 of file segbot_logical_translator.h.
ros::ServiceClient segbot_logical_translator::SegbotLogicalTranslator::make_plan_client_ [protected] |
Definition at line 147 of file segbot_logical_translator.h.
Definition at line 148 of file segbot_logical_translator.h.
nav_msgs::OccupancyGrid segbot_logical_translator::SegbotLogicalTranslator::map_ [protected] |
Definition at line 141 of file segbot_logical_translator.h.
nav_msgs::OccupancyGrid segbot_logical_translator::SegbotLogicalTranslator::map_with_doors_ [protected] |
Definition at line 142 of file segbot_logical_translator.h.
boost::shared_ptr<ros::NodeHandle> segbot_logical_translator::SegbotLogicalTranslator::nh_ [protected] |
Definition at line 146 of file segbot_logical_translator.h.
std::map<std::string, geometry_msgs::Pose> segbot_logical_translator::SegbotLogicalTranslator::object_approach_map_ [protected] |
Definition at line 138 of file segbot_logical_translator.h.
std::map<std::string, boost::shared_ptr<bwi_mapper::PathFinder> > segbot_logical_translator::SegbotLogicalTranslator::object_approachable_space_ [protected] |
Definition at line 139 of file segbot_logical_translator.h.