$search
#include <OctomapServer.h>
Public Types | |
typedef pcl::PointCloud < pcl::PointXYZ > | PCLPointCloud |
Public Member Functions | |
bool | clearBBXSrv (octomap_ros::ClearBBXRegionRequest &req, octomap_ros::ClearBBXRegionRequest &resp) |
void | insertCloudCallback (const sensor_msgs::PointCloud2::ConstPtr &cloud) |
OctomapServer (const std::string &filename="") | |
bool | openFile (const std::string &filename) |
bool | resetSrv (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) |
bool | serviceCallback (octomap_ros::GetOctomap::Request &req, octomap_ros::GetOctomap::Response &res) |
virtual | ~OctomapServer () |
Protected Member Functions | |
void | filterGroundPlane (const PCLPointCloud &pc, PCLPointCloud &ground, PCLPointCloud &nonground) const |
label the input cloud "pc" into ground and nonground. Should be in the robot's fixed frame (not world!) | |
void | handleFreeNode (const octomap::OcTreeROS::OcTreeType::iterator &it) |
hook that is called when traversing free nodes of the updated Octree (updates 2D map projection here) | |
void | handleNode (const octomap::OcTreeROS::OcTreeType::iterator &it) |
hook that is called when traversing all nodes of the updated Octree (does nothing here) | |
void | handleOccupiedNode (const octomap::OcTreeROS::OcTreeType::iterator &it) |
hook that is called when traversing occupied nodes of the updated Octree (updates 2D map projection here) | |
void | handlePostNodeTraversal (const ros::Time &rostime) |
hook that is called after traversing all nodes | |
void | handlePreNodeTraversal (const ros::Time &rostime) |
hook that is called after traversing all nodes | |
std_msgs::ColorRGBA | heightMapColor (double h) const |
virtual void | insertScan (const tf::Point &sensorOrigin, const PCLPointCloud &ground, const PCLPointCloud &nonground) |
update occupancy map with a scan labeled as ground and nonground. The scans should be in the global map frame. | |
bool | isSpeckleNode (const octomap::OcTreeKey &key) const |
Find speckle nodes (single occupied voxels with no neighbors). Only works on lowest resolution! | |
void | publishAll (const ros::Time &rostime=ros::Time::now()) |
void | publishMap (const ros::Time &rostime=ros::Time::now()) const |
void | reconfigureCallback (octomap_server::OctomapServerConfig &config, uint32_t level) |
Protected Attributes | |
std::string | m_baseFrameId |
ros::Publisher | m_binaryMapPub |
ros::ServiceServer | m_clearBBXService |
ros::Publisher | m_cmapPub |
ros::Publisher | m_collisionObjectPub |
std_msgs::ColorRGBA | m_color |
double | m_colorFactor |
bool | m_filterGroundPlane |
bool | m_filterSpeckles |
nav_msgs::OccupancyGrid | m_gridmap |
double | m_groundFilterAngle |
double | m_groundFilterDistance |
double | m_groundFilterPlaneDistance |
octomap::KeyRay | m_keyRay |
bool | m_latchedTopics |
ros::Publisher | m_mapPub |
ros::Publisher | m_markerPub |
double | m_maxRange |
unsigned | m_maxTreeDepth |
double | m_minSizeX |
double | m_minSizeY |
unsigned | m_multires2DScale |
ros::NodeHandle | m_nh |
double | m_occupancyMaxZ |
double | m_occupancyMinZ |
octomap::OcTreeROS * | m_octoMap |
ros::ServiceServer | m_octomapService |
octomap::OcTreeKey | m_paddedMinKey |
double | m_pointcloudMaxZ |
double | m_pointcloudMinZ |
ros::Publisher | m_pointCloudPub |
message_filters::Subscriber < sensor_msgs::PointCloud2 > * | m_pointCloudSub |
double | m_probHit |
double | m_probMiss |
bool | m_publish2DMap |
dynamic_reconfigure::Server < OctomapServerConfig > | m_reconfigureServer |
double | m_res |
ros::ServiceServer | m_resetService |
tf::TransformListener | m_tfListener |
tf::MessageFilter < sensor_msgs::PointCloud2 > * | m_tfPointCloudSub |
double | m_thresMax |
double | m_thresMin |
unsigned | m_treeDepth |
bool | m_useHeightMap |
std::string | m_worldFrameId |
Definition at line 72 of file OctomapServer.h.
typedef pcl::PointCloud<pcl::PointXYZ> octomap_server::OctomapServer::PCLPointCloud |
Definition at line 75 of file OctomapServer.h.
octomap_server::OctomapServer::OctomapServer | ( | const std::string & | filename = "" |
) |
Definition at line 44 of file OctomapServer.cpp.
octomap_server::OctomapServer::~OctomapServer | ( | ) | [virtual] |
Definition at line 161 of file OctomapServer.cpp.
bool octomap_server::OctomapServer::clearBBXSrv | ( | octomap_ros::ClearBBXRegionRequest & | req, | |
octomap_ros::ClearBBXRegionRequest & | resp | |||
) |
Definition at line 524 of file OctomapServer.cpp.
void octomap_server::OctomapServer::filterGroundPlane | ( | const PCLPointCloud & | pc, | |
PCLPointCloud & | ground, | |||
PCLPointCloud & | nonground | |||
) | const [protected] |
label the input cloud "pc" into ground and nonground. Should be in the robot's fixed frame (not world!)
Definition at line 578 of file OctomapServer.cpp.
void octomap_server::OctomapServer::handleFreeNode | ( | const octomap::OcTreeROS::OcTreeType::iterator & | it | ) | [protected] |
hook that is called when traversing free nodes of the updated Octree (updates 2D map projection here)
Reimplemented in octomap_server::OctomapServerMultilayer.
void octomap_server::OctomapServer::handleNode | ( | const octomap::OcTreeROS::OcTreeType::iterator & | it | ) | [inline, protected] |
hook that is called when traversing all nodes of the updated Octree (does nothing here)
Reimplemented in octomap_server::OctomapServerMultilayer.
Definition at line 116 of file OctomapServer.h.
void octomap_server::OctomapServer::handleOccupiedNode | ( | const octomap::OcTreeROS::OcTreeType::iterator & | it | ) | [protected] |
hook that is called when traversing occupied nodes of the updated Octree (updates 2D map projection here)
Reimplemented in octomap_server::OctomapServerMultilayer.
void octomap_server::OctomapServer::handlePostNodeTraversal | ( | const ros::Time & | rostime | ) | [protected] |
hook that is called after traversing all nodes
Reimplemented in octomap_server::OctomapServerMultilayer.
Definition at line 783 of file OctomapServer.cpp.
void octomap_server::OctomapServer::handlePreNodeTraversal | ( | const ros::Time & | rostime | ) | [protected] |
hook that is called after traversing all nodes
Reimplemented in octomap_server::OctomapServerMultilayer.
Definition at line 687 of file OctomapServer.cpp.
std_msgs::ColorRGBA octomap_server::OctomapServer::heightMapColor | ( | double | h | ) | const [protected] |
Definition at line 872 of file OctomapServer.cpp.
void octomap_server::OctomapServer::insertCloudCallback | ( | const sensor_msgs::PointCloud2::ConstPtr & | cloud | ) |
Definition at line 191 of file OctomapServer.cpp.
void octomap_server::OctomapServer::insertScan | ( | const tf::Point & | sensorOrigin, | |
const PCLPointCloud & | ground, | |||
const PCLPointCloud & | nonground | |||
) | [protected, virtual] |
update occupancy map with a scan labeled as ground and nonground. The scans should be in the global map frame.
sensorOrigin | origin of the measurements for raycasting | |
ground | scan endpoints on the ground plane (only clear space) | |
nonground | all other endpoints (clear up to occupied endpoint) |
Reimplemented in octomap_server::TrackingOctomapServer.
Definition at line 271 of file OctomapServer.cpp.
bool octomap_server::OctomapServer::isSpeckleNode | ( | const octomap::OcTreeKey & | key | ) | const [protected] |
Find speckle nodes (single occupied voxels with no neighbors). Only works on lowest resolution!
key |
bool octomap_server::OctomapServer::openFile | ( | const std::string & | filename | ) |
Definition at line 173 of file OctomapServer.cpp.
void octomap_server::OctomapServer::publishAll | ( | const ros::Time & | rostime = ros::Time::now() |
) | [protected] |
Definition at line 336 of file OctomapServer.cpp.
void octomap_server::OctomapServer::publishMap | ( | const ros::Time & | rostime = ros::Time::now() |
) | const [protected] |
Definition at line 566 of file OctomapServer.cpp.
void octomap_server::OctomapServer::reconfigureCallback | ( | octomap_server::OctomapServerConfig & | config, | |
uint32_t | level | |||
) | [protected] |
Definition at line 860 of file OctomapServer.cpp.
bool octomap_server::OctomapServer::resetSrv | ( | std_srvs::Empty::Request & | req, | |
std_srvs::Empty::Response & | resp | |||
) |
Definition at line 543 of file OctomapServer.cpp.
bool octomap_server::OctomapServer::serviceCallback | ( | octomap_ros::GetOctomap::Request & | req, | |
octomap_ros::GetOctomap::Response & | res | |||
) |
Definition at line 513 of file OctomapServer.cpp.
std::string octomap_server::OctomapServer::m_baseFrameId [protected] |
Definition at line 140 of file OctomapServer.h.
Definition at line 129 of file OctomapServer.h.
Definition at line 132 of file OctomapServer.h.
Definition at line 129 of file OctomapServer.h.
Definition at line 129 of file OctomapServer.h.
std_msgs::ColorRGBA octomap_server::OctomapServer::m_color [protected] |
Definition at line 142 of file OctomapServer.h.
double octomap_server::OctomapServer::m_colorFactor [protected] |
Definition at line 143 of file OctomapServer.h.
bool octomap_server::OctomapServer::m_filterGroundPlane [protected] |
Definition at line 163 of file OctomapServer.h.
bool octomap_server::OctomapServer::m_filterSpeckles [protected] |
Definition at line 161 of file OctomapServer.h.
Definition at line 169 of file OctomapServer.h.
double octomap_server::OctomapServer::m_groundFilterAngle [protected] |
Definition at line 165 of file OctomapServer.h.
double octomap_server::OctomapServer::m_groundFilterDistance [protected] |
Definition at line 164 of file OctomapServer.h.
double octomap_server::OctomapServer::m_groundFilterPlaneDistance [protected] |
Definition at line 166 of file OctomapServer.h.
octomap::KeyRay octomap_server::OctomapServer::m_keyRay [protected] |
Definition at line 137 of file OctomapServer.h.
bool octomap_server::OctomapServer::m_latchedTopics [protected] |
Definition at line 145 of file OctomapServer.h.
Definition at line 129 of file OctomapServer.h.
Definition at line 129 of file OctomapServer.h.
double octomap_server::OctomapServer::m_maxRange [protected] |
Definition at line 138 of file OctomapServer.h.
unsigned octomap_server::OctomapServer::m_maxTreeDepth [protected] |
Definition at line 149 of file OctomapServer.h.
double octomap_server::OctomapServer::m_minSizeX [protected] |
Definition at line 159 of file OctomapServer.h.
double octomap_server::OctomapServer::m_minSizeY [protected] |
Definition at line 160 of file OctomapServer.h.
unsigned octomap_server::OctomapServer::m_multires2DScale [protected] |
Definition at line 172 of file OctomapServer.h.
ros::NodeHandle octomap_server::OctomapServer::m_nh [protected] |
Definition at line 128 of file OctomapServer.h.
double octomap_server::OctomapServer::m_occupancyMaxZ [protected] |
Definition at line 158 of file OctomapServer.h.
double octomap_server::OctomapServer::m_occupancyMinZ [protected] |
Definition at line 157 of file OctomapServer.h.
Definition at line 136 of file OctomapServer.h.
Definition at line 132 of file OctomapServer.h.
octomap::OcTreeKey octomap_server::OctomapServer::m_paddedMinKey [protected] |
Definition at line 171 of file OctomapServer.h.
double octomap_server::OctomapServer::m_pointcloudMaxZ [protected] |
Definition at line 156 of file OctomapServer.h.
double octomap_server::OctomapServer::m_pointcloudMinZ [protected] |
Definition at line 155 of file OctomapServer.h.
Definition at line 129 of file OctomapServer.h.
message_filters::Subscriber<sensor_msgs::PointCloud2>* octomap_server::OctomapServer::m_pointCloudSub [protected] |
Definition at line 130 of file OctomapServer.h.
double octomap_server::OctomapServer::m_probHit [protected] |
Definition at line 150 of file OctomapServer.h.
double octomap_server::OctomapServer::m_probMiss [protected] |
Definition at line 151 of file OctomapServer.h.
bool octomap_server::OctomapServer::m_publish2DMap [protected] |
Definition at line 170 of file OctomapServer.h.
dynamic_reconfigure::Server<OctomapServerConfig> octomap_server::OctomapServer::m_reconfigureServer [protected] |
Definition at line 134 of file OctomapServer.h.
double octomap_server::OctomapServer::m_res [protected] |
Definition at line 147 of file OctomapServer.h.
Definition at line 132 of file OctomapServer.h.
Definition at line 133 of file OctomapServer.h.
tf::MessageFilter<sensor_msgs::PointCloud2>* octomap_server::OctomapServer::m_tfPointCloudSub [protected] |
Definition at line 131 of file OctomapServer.h.
double octomap_server::OctomapServer::m_thresMax [protected] |
Definition at line 153 of file OctomapServer.h.
double octomap_server::OctomapServer::m_thresMin [protected] |
Definition at line 152 of file OctomapServer.h.
unsigned octomap_server::OctomapServer::m_treeDepth [protected] |
Definition at line 148 of file OctomapServer.h.
bool octomap_server::OctomapServer::m_useHeightMap [protected] |
Definition at line 141 of file OctomapServer.h.
std::string octomap_server::OctomapServer::m_worldFrameId [protected] |
Definition at line 139 of file OctomapServer.h.