53 , max_range_(
std::numeric_limits<double>::infinity())
56 , point_cloud_subscriber_(NULL)
57 , point_cloud_filter_(NULL)
70 if (!params.
hasMember(
"point_cloud_topic"))
80 if (params.
hasMember(
"filtered_cloud_topic"))
113 ROS_INFO(
"Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(),
119 ROS_INFO(
"Listening to '%s'", point_cloud_topic_.c_str());
142 ROS_ERROR(
"Shape filter not yet initialized!");
159 transform = it->second;
164 std::vector<int>& mask)
170 ROS_DEBUG(
"Received a new point cloud message");
199 ROS_ERROR_STREAM(
"Transform error of sensor data: " << ex.what() <<
"; quitting callback");
210 Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.
getX(), sensor_origin_tf.
getY(), sensor_origin_tf.
getZ());
223 std::unique_ptr<sensor_msgs::PointCloud2> filtered_cloud;
228 std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_x;
229 std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_y;
230 std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_z;
234 filtered_cloud.reset(
new sensor_msgs::PointCloud2());
235 filtered_cloud->header = cloud_msg->header;
238 pcd_modifier.
resize(cloud_msg->width * cloud_msg->height);
245 size_t filtered_cloud_size = 0;
253 for (
unsigned int row = 0; row < cloud_msg->height; row +=
point_subsample_)
255 unsigned int row_c = row * cloud_msg->width;
266 if (!std::isnan(pt_iter[0]) && !std::isnan(pt_iter[1]) && !std::isnan(pt_iter[2]))
274 model_cells.insert(
tree_->coordToKey(point_tf.
getX(), point_tf.
getY(), point_tf.
getZ()));
276 clip_cells.insert(
tree_->coordToKey(point_tf.
getX(), point_tf.
getY(), point_tf.
getZ()));
279 occupied_cells.insert(
tree_->coordToKey(point_tf.
getX(), point_tf.
getY(), point_tf.
getZ()));
283 **iter_filtered_x = pt_iter[0];
284 **iter_filtered_y = pt_iter[1];
285 **iter_filtered_z = pt_iter[2];
286 ++filtered_cloud_size;
297 for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
302 for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
307 for (octomap::KeySet::iterator it = clip_cells.begin(), end = clip_cells.end(); it != end; ++it)
320 for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
321 occupied_cells.erase(*it);
324 for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
325 free_cells.erase(*it);
332 for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it)
333 tree_->updateNode(*it,
false);
336 for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
337 tree_->updateNode(*it,
true);
340 const float lg =
tree_->getClampingThresMinLog() -
tree_->getClampingThresMaxLog();
341 for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
342 tree_->updateNode(*it, lg);
346 ROS_ERROR(
"Internal error while updating octree");
348 tree_->unlockWrite();
350 tree_->triggerUpdateCallback();
355 pcd_modifier.
resize(filtered_cloud_size);
const std::string & getMessage() const
virtual bool initialize()
Do any necessary setup (subscribe to ros topics, etc.). This call assumes setMonitor() and setParams(...
std::unique_ptr< point_containment_filter::ShapeMask > shape_mask_
OccupancyMapMonitor * monitor_
void publish(const boost::shared_ptr< M > &message) const
void setMapFrame(const std::string &frame)
std::string point_cloud_topic_
Base class for classes which update the occupancy map.
message_filters::Subscriber< sensor_msgs::PointCloud2 > * point_cloud_subscriber_
TFSIMD_FORCE_INLINE const tfScalar & getY() const
std::string filtered_cloud_topic_
virtual ~PointCloudOctomapUpdater()
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape)
boost::shared_ptr< tf::Transformer > tf_
bool getShapeTransform(ShapeHandle h, Eigen::Affine3d &transform) const
unsigned int point_subsample_
#define ROS_ERROR_THROTTLE(rate,...)
const std::string & getMapFrame() const
const boost::shared_ptr< tf::Transformer > & getTFClient() const
Computing a mask for a pointcloud that states which points are inside the robot.
virtual void updateMask(const sensor_msgs::PointCloud2 &cloud, const Eigen::Vector3d &sensor_origin, std::vector< int > &mask)
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
bool hasMember(const std::string &name) const
ros::Publisher filtered_cloud_publisher_
virtual void forgetShape(ShapeHandle handle)
static void readXmlParam(XmlRpc::XmlRpcValue ¶ms, const std::string ¶m_name, double *value)
virtual bool setParams(XmlRpc::XmlRpcValue ¶ms)
Set updater params using struct that comes from parsing a yaml string. This must be called after setM...
TFSIMD_FORCE_INLINE const tfScalar & getX() const
std::string getTargetFramesString()
ros::Time last_update_time_
bool updateTransformCache(const std::string &target_frame, const ros::Time &target_time)
#define ROS_ERROR_STREAM(args)
ShapeTransformCache transform_cache_
void setPointCloud2FieldsByString(int n_fields,...)
ros::NodeHandle private_nh_
PointCloudOctomapUpdater()
Connection registerCallback(const C &callback)
std::shared_ptr< const Shape > ShapeConstPtr
tf::MessageFilter< sensor_msgs::PointCloud2 > * point_cloud_filter_