38 #include <moveit_msgs/SaveMap.h> 39 #include <moveit_msgs/LoadMap.h> 47 : map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_(
"~"), active_(false)
53 double map_resolution)
60 const std::string& map_frame,
double map_resolution)
80 ROS_WARN(
"No target frame specified for Octomap. No transforms will be applied to received data.");
83 ROS_WARN(
"Target frame specified but no TF instance specified. No transforms will be applied to received data.");
94 for (int32_t i = 0; i < sensor_list.
size(); ++i)
98 ROS_ERROR(
"Params for octomap updater %d not a struct; ignoring.", i);
102 if (!sensor_list[i].hasMember(
"sensor_plugin"))
104 ROS_ERROR(
"No sensor plugin specified for octomap updater %d; ignoring.", i);
108 std::string sensor_plugin = std::string(sensor_list[i][
"sensor_plugin"]);
109 if (sensor_plugin.empty() || sensor_plugin[0] ==
'~')
111 ROS_INFO(
"Skipping octomap updater plugin '%s'", sensor_plugin.c_str());
120 "moveit_ros_perception",
"occupancy_map_monitor::OccupancyMapUpdater"));
124 ROS_FATAL_STREAM(
"Exception while creating octomap updater plugin loader " << ex.what());
128 OccupancyMapUpdaterPtr up;
132 up->setMonitor(
this);
136 ROS_ERROR_STREAM(
"Exception while loading octomap updater '" << sensor_plugin <<
"': " << ex.what()
142 if (!up->setParams(sensor_list[i]))
144 ROS_ERROR(
"Failed to configure updater of type %s", up->getType().c_str());
148 if (!up->initialize())
150 ROS_ERROR(
"Unable to initialize map updater of type %s (plugin %s)", up->getType().c_str(),
151 sensor_plugin.c_str());
159 ROS_ERROR(
"List of sensors must be an array!");
244 std::map<ShapeHandle, ShapeHandle>::const_iterator it =
mesh_handles_[i].find(handle);
268 for (ShapeTransformCache::iterator it = tempCache.begin(); it != tempCache.end(); ++it)
270 std::map<ShapeHandle, ShapeHandle>::const_iterator jt =
mesh_handles_[index].find(it->first);
277 cache[jt->second] = it->second;
289 moveit_msgs::SaveMap::Response& response)
291 ROS_INFO(
"Writing map to %s", request.filename.c_str());
295 response.success =
tree_->writeBinary(request.filename);
299 response.success =
false;
306 moveit_msgs::LoadMap::Response& response)
308 ROS_INFO(
"Reading map from %s", request.filename.c_str());
314 response.success =
tree_->readBinary(request.filename);
318 ROS_ERROR(
"Failed to load map from file");
319 response.success =
false;
321 tree_->unlockWrite();
const std::string & getMessage() const
std::map< ShapeHandle, Eigen::Affine3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Affine3d > > > ShapeTransformCache
void setTransformCacheCallback(const TransformCacheProvider &transform_cache_callback)
ros::ServiceServer save_map_srv_
ros::ServiceServer load_map_srv_
void setMapFrame(const std::string &frame)
std::unique_ptr< pluginlib::ClassLoader< OccupancyMapUpdater > > updater_plugin_loader_
std::size_t mesh_handle_count_
boost::shared_ptr< tf::Transformer > tf_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
std::vector< std::map< ShapeHandle, ShapeHandle > > mesh_handles_
bool loadMapCallback(moveit_msgs::LoadMap::Request &request, moveit_msgs::LoadMap::Response &response)
Load octree from a binary file (gets rid of current octree data)
boost::mutex parameters_lock_
void publishDebugInformation(bool flag)
#define ROS_ERROR_THROTTLE(rate,...)
bool saveMapCallback(moveit_msgs::SaveMap::Request &request, moveit_msgs::SaveMap::Response &response)
Save the current octree to a binary file.
#define ROS_FATAL_STREAM(args)
void forgetShape(ShapeHandle handle)
Forget about this shape handle and the shapes it corresponds to.
OccMapTreeConstPtr tree_const_
void addUpdater(const OccupancyMapUpdaterPtr &updater)
void startMonitor()
start the monitor (will begin updating the octomap
TransformCacheProvider transform_cache_callback_
boost::function< bool(const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache)> TransformCacheProvider
std::vector< OccupancyMapUpdaterPtr > map_updaters_
bool getShapeTransformCache(std::size_t index, const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache) const
bool getParam(const std::string &key, std::string &s) const
OccupancyMapMonitor(const boost::shared_ptr< tf::Transformer > &tf, const std::string &map_frame="", double map_resolution=0.0)
ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape)
Add this shape to the set of shapes to be filtered out from the octomap.
#define ROS_ERROR_STREAM(args)
std::shared_ptr< const Shape > ShapeConstPtr