#include <MapHelper.hpp>
Public Member Functions | |
bool | doRaytracing (const SimpleVector3 &fromPoint, const SimpleVector3 &toPoint) |
bool | doRaytracing (const SimpleVector3 &fromPoint, const SimpleVector3 &toPoint, Ray &ray) |
int8_t | getCollisionThreshold () |
int8_t | getCostmapOccupancyValue (const SimpleVector3 &position) |
int8_t | getCostmapOccupancyValue (const int32_t &mapX, const int32_t &mapY) |
int8_t | getMapOccupancyValue (const SimpleVector3 &position) |
int8_t | getMapOccupancyValue (const int32_t &mapX, const int32_t &mapY) |
float | getMetricHeight () |
float | getMetricWidth () |
SimpleQuaternion | getOrientation () |
int8_t | getRaytracingMapOccupancyValue (const SimpleVector3 &position) |
int8_t | getRaytracingMapOccupancyValue (const int32_t &mapX, const int32_t &mapY) |
float | getSquaredDistance (const SimpleVector3 &start, const SimpleVector3 &goal) |
SimpleVector3 | getTranslation () |
bool | isOccupancyValueAcceptable (const int8_t &occupancyValue) |
MapHelper (const std::string &mapTopicName="map", const std::string &getPlanServiceName="move_base/make_plan") | |
void | mapToWorldCoordinates (const int32_t &x, const int32_t &y, SimpleVector3 &result) |
void | mapToWorldCoordinates (const SimpleVector3 &position, SimpleVector3 &result) |
void | mapToWorldSize (const double &size, double &result) |
void | setCollisionThreshold (int8_t thresholdValue) |
void | worldToMapCoordinates (const SimpleVector3 &position, int32_t &x, int32_t &y) |
void | worldToMapCoordinates (const SimpleVector3 &position, SimpleVector3 &result) |
void | worldToMapSize (const double &size, double &result) |
virtual | ~MapHelper () |
Private Member Functions | |
void | aggregateRaytracingMap () |
int8_t | getOccupancyValue (const nav_msgs::OccupancyGrid &map, const SimpleVector3 &position) |
int8_t | getOccupancyValue (const costmap_2d::Costmap2D &costmap, const SimpleVector3 &position) |
int8_t | getOccupancyValue (const nav_msgs::OccupancyGrid &map, const int32_t &mapX, const int32_t &mapY) |
int8_t | getOccupancyValue (const costmap_2d::Costmap2D &costmap, const int32_t &mapX, const int32_t &mapY) |
void | mapReceived (nav_msgs::OccupancyGrid map) |
void | setCostmap () |
Private Attributes | |
int8_t | mCollisionThreshold |
costmap_2d::Costmap2D | mCostmap |
int8_t | mCostTranslationTable [256] |
DebugHelperPtr | mDebugHelperPtr |
ros::ServiceClient | mGetPlanServiceClient |
ros::NodeHandle | mGlobalNodeHandle |
nav_msgs::OccupancyGrid | mMap |
bool | mMapReceived |
nav_msgs::OccupancyGrid | mRaytracingMap |
Definition at line 62 of file MapHelper.hpp.
robot_model_services::MapHelper::MapHelper | ( | const std::string & | mapTopicName = "map" , |
const std::string & | getPlanServiceName = "move_base/make_plan" |
||
) | [inline] |
Definition at line 76 of file MapHelper.hpp.
virtual robot_model_services::MapHelper::~MapHelper | ( | ) | [inline, virtual] |
Definition at line 111 of file MapHelper.hpp.
void robot_model_services::MapHelper::aggregateRaytracingMap | ( | ) | [inline, private] |
Definition at line 149 of file MapHelper.hpp.
bool robot_model_services::MapHelper::doRaytracing | ( | const SimpleVector3 & | fromPoint, |
const SimpleVector3 & | toPoint | ||
) | [inline] |
Definition at line 235 of file MapHelper.hpp.
bool robot_model_services::MapHelper::doRaytracing | ( | const SimpleVector3 & | fromPoint, |
const SimpleVector3 & | toPoint, | ||
Ray & | ray | ||
) | [inline] |
Definition at line 241 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getCollisionThreshold | ( | ) | [inline] |
Definition at line 396 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getCostmapOccupancyValue | ( | const SimpleVector3 & | position | ) | [inline] |
Definition at line 218 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getCostmapOccupancyValue | ( | const int32_t & | mapX, |
const int32_t & | mapY | ||
) | [inline] |
Definition at line 222 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getMapOccupancyValue | ( | const SimpleVector3 & | position | ) | [inline] |
Definition at line 209 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getMapOccupancyValue | ( | const int32_t & | mapX, |
const int32_t & | mapY | ||
) | [inline] |
Definition at line 213 of file MapHelper.hpp.
float robot_model_services::MapHelper::getMetricHeight | ( | ) | [inline] |
Definition at line 340 of file MapHelper.hpp.
float robot_model_services::MapHelper::getMetricWidth | ( | ) | [inline] |
Definition at line 336 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getOccupancyValue | ( | const nav_msgs::OccupancyGrid & | map, |
const SimpleVector3 & | position | ||
) | [inline, private] |
Definition at line 173 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getOccupancyValue | ( | const costmap_2d::Costmap2D & | costmap, |
const SimpleVector3 & | position | ||
) | [inline, private] |
Definition at line 179 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getOccupancyValue | ( | const nav_msgs::OccupancyGrid & | map, |
const int32_t & | mapX, | ||
const int32_t & | mapY | ||
) | [inline, private] |
Definition at line 185 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getOccupancyValue | ( | const costmap_2d::Costmap2D & | costmap, |
const int32_t & | mapX, | ||
const int32_t & | mapY | ||
) | [inline, private] |
Definition at line 196 of file MapHelper.hpp.
Definition at line 384 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getRaytracingMapOccupancyValue | ( | const SimpleVector3 & | position | ) | [inline] |
Definition at line 227 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::getRaytracingMapOccupancyValue | ( | const int32_t & | mapX, |
const int32_t & | mapY | ||
) | [inline] |
Definition at line 231 of file MapHelper.hpp.
float robot_model_services::MapHelper::getSquaredDistance | ( | const SimpleVector3 & | start, |
const SimpleVector3 & | goal | ||
) | [inline] |
Definition at line 400 of file MapHelper.hpp.
Definition at line 380 of file MapHelper.hpp.
bool robot_model_services::MapHelper::isOccupancyValueAcceptable | ( | const int8_t & | occupancyValue | ) | [inline] |
Definition at line 388 of file MapHelper.hpp.
void robot_model_services::MapHelper::mapReceived | ( | nav_msgs::OccupancyGrid | map | ) | [inline, private] |
Definition at line 113 of file MapHelper.hpp.
void robot_model_services::MapHelper::mapToWorldCoordinates | ( | const int32_t & | x, |
const int32_t & | y, | ||
SimpleVector3 & | result | ||
) | [inline] |
Definition at line 362 of file MapHelper.hpp.
void robot_model_services::MapHelper::mapToWorldCoordinates | ( | const SimpleVector3 & | position, |
SimpleVector3 & | result | ||
) | [inline] |
Definition at line 366 of file MapHelper.hpp.
void robot_model_services::MapHelper::mapToWorldSize | ( | const double & | size, |
double & | result | ||
) | [inline] |
Definition at line 372 of file MapHelper.hpp.
void robot_model_services::MapHelper::setCollisionThreshold | ( | int8_t | thresholdValue | ) | [inline] |
Definition at line 392 of file MapHelper.hpp.
void robot_model_services::MapHelper::setCostmap | ( | ) | [inline, private] |
Definition at line 128 of file MapHelper.hpp.
void robot_model_services::MapHelper::worldToMapCoordinates | ( | const SimpleVector3 & | position, |
int32_t & | x, | ||
int32_t & | y | ||
) | [inline] |
Definition at line 344 of file MapHelper.hpp.
void robot_model_services::MapHelper::worldToMapCoordinates | ( | const SimpleVector3 & | position, |
SimpleVector3 & | result | ||
) | [inline] |
Definition at line 353 of file MapHelper.hpp.
void robot_model_services::MapHelper::worldToMapSize | ( | const double & | size, |
double & | result | ||
) | [inline] |
Definition at line 376 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::mCollisionThreshold [private] |
Definition at line 67 of file MapHelper.hpp.
Definition at line 71 of file MapHelper.hpp.
int8_t robot_model_services::MapHelper::mCostTranslationTable[256] [private] |
Definition at line 74 of file MapHelper.hpp.
Definition at line 64 of file MapHelper.hpp.
Definition at line 69 of file MapHelper.hpp.
Definition at line 68 of file MapHelper.hpp.
nav_msgs::OccupancyGrid robot_model_services::MapHelper::mMap [private] |
Definition at line 70 of file MapHelper.hpp.
bool robot_model_services::MapHelper::mMapReceived [private] |
Definition at line 66 of file MapHelper.hpp.
nav_msgs::OccupancyGrid robot_model_services::MapHelper::mRaytracingMap [private] |
Definition at line 72 of file MapHelper.hpp.