#include <occupancy_map_monitor.h>
Public Member Functions | |
void | addUpdater (const OccupancyMapUpdaterPtr &updater) |
ShapeHandle | excludeShape (const shapes::ShapeConstPtr &shape) |
Add this shape to the set of shapes to be filtered out from the octomap. More... | |
void | forgetShape (ShapeHandle handle) |
Forget about this shape handle and the shapes it corresponds to. More... | |
const std::string & | getMapFrame () const |
double | getMapResolution () const |
const OccMapTreePtr & | getOcTreePtr () |
Get a pointer to the underlying octree for this monitor. Lock the tree before reading or writing using this pointer. The value of this pointer stays the same throughout the existance of the monitor instance. More... | |
const OccMapTreeConstPtr & | getOcTreePtr () const |
Get a const pointer to the underlying octree for this monitor. Lock the tree before reading this pointer. More... | |
const boost::shared_ptr< tf::Transformer > & | getTFClient () const |
bool | isActive () const |
OccupancyMapMonitor (const boost::shared_ptr< tf::Transformer > &tf, const std::string &map_frame="", double map_resolution=0.0) | |
OccupancyMapMonitor (double map_resolution=0.0) | |
OccupancyMapMonitor (const boost::shared_ptr< tf::Transformer > &tf, ros::NodeHandle &nh, const std::string &map_frame="", double map_resolution=0.0) | |
void | publishDebugInformation (bool flag) |
void | setMapFrame (const std::string &frame) |
void | setTransformCacheCallback (const TransformCacheProvider &transform_cache_callback) |
void | setUpdateCallback (const boost::function< void()> &update_callback) |
Set the callback to trigger when updates to the maintained octomap are received. More... | |
void | startMonitor () |
start the monitor (will begin updating the octomap More... | |
void | stopMonitor () |
~OccupancyMapMonitor () | |
Private Member Functions | |
bool | getShapeTransformCache (std::size_t index, const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache) const |
void | initialize () |
bool | loadMapCallback (moveit_msgs::LoadMap::Request &request, moveit_msgs::LoadMap::Response &response) |
Load octree from a binary file (gets rid of current octree data) More... | |
bool | saveMapCallback (moveit_msgs::SaveMap::Request &request, moveit_msgs::SaveMap::Response &response) |
Save the current octree to a binary file. More... | |
Private Attributes | |
bool | active_ |
bool | debug_info_ |
ros::ServiceServer | load_map_srv_ |
std::string | map_frame_ |
double | map_resolution_ |
std::vector< OccupancyMapUpdaterPtr > | map_updaters_ |
std::size_t | mesh_handle_count_ |
std::vector< std::map< ShapeHandle, ShapeHandle > > | mesh_handles_ |
ros::NodeHandle | nh_ |
boost::mutex | parameters_lock_ |
ros::NodeHandle | root_nh_ |
ros::ServiceServer | save_map_srv_ |
boost::shared_ptr< tf::Transformer > | tf_ |
TransformCacheProvider | transform_cache_callback_ |
OccMapTreePtr | tree_ |
OccMapTreeConstPtr | tree_const_ |
std::unique_ptr< pluginlib::ClassLoader< OccupancyMapUpdater > > | updater_plugin_loader_ |
Definition at line 57 of file occupancy_map_monitor.h.
occupancy_map_monitor::OccupancyMapMonitor::OccupancyMapMonitor | ( | const boost::shared_ptr< tf::Transformer > & | tf, |
const std::string & | map_frame = "" , |
||
double | map_resolution = 0.0 |
||
) |
Definition at line 52 of file occupancy_map_monitor.cpp.
occupancy_map_monitor::OccupancyMapMonitor::OccupancyMapMonitor | ( | double | map_resolution = 0.0 | ) |
Definition at line 46 of file occupancy_map_monitor.cpp.
occupancy_map_monitor::OccupancyMapMonitor::OccupancyMapMonitor | ( | const boost::shared_ptr< tf::Transformer > & | tf, |
ros::NodeHandle & | nh, | ||
const std::string & | map_frame = "" , |
||
double | map_resolution = 0.0 |
||
) |
Definition at line 59 of file occupancy_map_monitor.cpp.
occupancy_map_monitor::OccupancyMapMonitor::~OccupancyMapMonitor | ( | ) |
Definition at line 341 of file occupancy_map_monitor.cpp.
void occupancy_map_monitor::OccupancyMapMonitor::addUpdater | ( | const OccupancyMapUpdaterPtr & | updater | ) |
Definition at line 172 of file occupancy_map_monitor.cpp.
ShapeHandle occupancy_map_monitor::OccupancyMapMonitor::excludeShape | ( | const shapes::ShapeConstPtr & | shape | ) |
Add this shape to the set of shapes to be filtered out from the octomap.
Definition at line 213 of file occupancy_map_monitor.cpp.
void occupancy_map_monitor::OccupancyMapMonitor::forgetShape | ( | ShapeHandle | handle | ) |
Forget about this shape handle and the shapes it corresponds to.
Definition at line 233 of file occupancy_map_monitor.cpp.
|
inline |
Definition at line 87 of file occupancy_map_monitor.h.
|
inline |
Definition at line 94 of file occupancy_map_monitor.h.
|
inline |
Get a pointer to the underlying octree for this monitor. Lock the tree before reading or writing using this pointer. The value of this pointer stays the same throughout the existance of the monitor instance.
Definition at line 75 of file occupancy_map_monitor.h.
|
inline |
Get a const pointer to the underlying octree for this monitor. Lock the tree before reading this pointer.
Definition at line 82 of file occupancy_map_monitor.h.
|
private |
Definition at line 260 of file occupancy_map_monitor.cpp.
|
inline |
Definition at line 99 of file occupancy_map_monitor.h.
|
private |
Definition at line 66 of file occupancy_map_monitor.cpp.
|
inline |
Definition at line 122 of file occupancy_map_monitor.h.
|
private |
Load octree from a binary file (gets rid of current octree data)
Definition at line 305 of file occupancy_map_monitor.cpp.
void occupancy_map_monitor::OccupancyMapMonitor::publishDebugInformation | ( | bool | flag | ) |
Definition at line 200 of file occupancy_map_monitor.cpp.
|
private |
Save the current octree to a binary file.
Definition at line 288 of file occupancy_map_monitor.cpp.
void occupancy_map_monitor::OccupancyMapMonitor::setMapFrame | ( | const std::string & | frame | ) |
Definition at line 207 of file occupancy_map_monitor.cpp.
void occupancy_map_monitor::OccupancyMapMonitor::setTransformCacheCallback | ( | const TransformCacheProvider & | transform_cache_callback | ) |
Definition at line 251 of file occupancy_map_monitor.cpp.
|
inline |
Set the callback to trigger when updates to the maintained octomap are received.
Definition at line 113 of file occupancy_map_monitor.h.
void occupancy_map_monitor::OccupancyMapMonitor::startMonitor | ( | ) |
start the monitor (will begin updating the octomap
Definition at line 326 of file occupancy_map_monitor.cpp.
void occupancy_map_monitor::OccupancyMapMonitor::stopMonitor | ( | ) |
Definition at line 334 of file occupancy_map_monitor.cpp.
|
private |
Definition at line 160 of file occupancy_map_monitor.h.
|
private |
Definition at line 151 of file occupancy_map_monitor.h.
|
private |
Definition at line 158 of file occupancy_map_monitor.h.
|
private |
Definition at line 140 of file occupancy_map_monitor.h.
|
private |
Definition at line 141 of file occupancy_map_monitor.h.
|
private |
Definition at line 148 of file occupancy_map_monitor.h.
|
private |
Definition at line 153 of file occupancy_map_monitor.h.
|
private |
Definition at line 149 of file occupancy_map_monitor.h.
|
private |
Definition at line 156 of file occupancy_map_monitor.h.
|
private |
Definition at line 142 of file occupancy_map_monitor.h.
|
private |
Definition at line 155 of file occupancy_map_monitor.h.
|
private |
Definition at line 157 of file occupancy_map_monitor.h.
|
private |
Definition at line 139 of file occupancy_map_monitor.h.
|
private |
Definition at line 150 of file occupancy_map_monitor.h.
|
private |
Definition at line 144 of file occupancy_map_monitor.h.
|
private |
Definition at line 145 of file occupancy_map_monitor.h.
|
private |
Definition at line 147 of file occupancy_map_monitor.h.