pointcloud_octomap_updater.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Jon Binney, Ioan Sucan */
36 
37 #pragma once
38 
39 #include <ros/ros.h>
41 #include <tf2_ros/message_filter.h>
43 #include <sensor_msgs/PointCloud2.h>
46 
47 #include <memory>
48 
49 namespace occupancy_map_monitor
50 {
51 class PointCloudOctomapUpdater : public OccupancyMapUpdater
52 {
53 public:
55  ~PointCloudOctomapUpdater() override;
56 
57  bool setParams(XmlRpc::XmlRpcValue& params) override;
58 
59  bool initialize() override;
60  void start() override;
61  void stop() override;
62  ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) override;
63  void forgetShape(ShapeHandle handle) override;
64 
65 protected:
66  virtual void updateMask(const sensor_msgs::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin,
67  std::vector<int>& mask);
68 
69 private:
70  bool getShapeTransform(ShapeHandle h, Eigen::Isometry3d& transform) const;
71  void cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
72  void stopHelper();
73 
76 
77  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
78  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
79 
81 
82  /* params */
83  std::string point_cloud_topic_;
84  double scale_;
85  double padding_;
86  double max_range_;
87  unsigned int point_subsample_;
88  double max_update_rate_;
89  std::string filtered_cloud_topic_;
90  std::string ns_;
92 
95 
96  /* used to store all cells in the map which a given ray passes through during raycasting.
97  we cache this here because it dynamically pre-allocates a lot of memory in its contsructor */
99 
100  std::unique_ptr<point_containment_filter::ShapeMask> shape_mask_;
101  std::vector<int> mask_;
102 };
103 } // namespace occupancy_map_monitor
occupancy_map_monitor::PointCloudOctomapUpdater::start
void start() override
Definition: pointcloud_octomap_updater.cpp:146
occupancy_map_monitor::PointCloudOctomapUpdater::forgetShape
void forgetShape(ShapeHandle handle) override
Definition: pointcloud_octomap_updater.cpp:192
occupancy_map_monitor::PointCloudOctomapUpdater::filtered_cloud_publisher_
ros::Publisher filtered_cloud_publisher_
Definition: pointcloud_octomap_updater.h:155
ros::Publisher
tf2_ros::MessageFilter< sensor_msgs::PointCloud2 >
ros.h
occupancy_map_monitor::PointCloudOctomapUpdater::~PointCloudOctomapUpdater
~PointCloudOctomapUpdater() override
Definition: pointcloud_octomap_updater.cpp:96
occupancy_map_monitor::PointCloudOctomapUpdater::stopHelper
void stopHelper()
Definition: pointcloud_octomap_updater.cpp:169
occupancy_map_monitor::PointCloudOctomapUpdater::scale_
double scale_
Definition: pointcloud_octomap_updater.h:148
occupancy_map_monitor::PointCloudOctomapUpdater::mask_
std::vector< int > mask_
Definition: pointcloud_octomap_updater.h:165
occupancy_map_monitor::PointCloudOctomapUpdater::point_cloud_topic_
std::string point_cloud_topic_
Definition: pointcloud_octomap_updater.h:147
occupancy_map_monitor::PointCloudOctomapUpdater::tf_listener_
std::shared_ptr< tf2_ros::TransformListener > tf_listener_
Definition: pointcloud_octomap_updater.h:142
octomap::KeyRay
occupancy_map_monitor::PointCloudOctomapUpdater::excludeShape
ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape) override
Definition: pointcloud_octomap_updater.cpp:182
message_filters::Subscriber< sensor_msgs::PointCloud2 >
occupancy_map_monitor::PointCloudOctomapUpdater::shape_mask_
std::unique_ptr< point_containment_filter::ShapeMask > shape_mask_
Definition: pointcloud_octomap_updater.h:164
occupancy_map_monitor::PointCloudOctomapUpdater::private_nh_
ros::NodeHandle private_nh_
Definition: pointcloud_octomap_updater.h:139
message_filter.h
subscriber.h
occupancy_map_monitor::PointCloudOctomapUpdater::point_cloud_subscriber_
message_filters::Subscriber< sensor_msgs::PointCloud2 > * point_cloud_subscriber_
Definition: pointcloud_octomap_updater.h:157
occupancy_map_monitor::PointCloudOctomapUpdater::ns_
std::string ns_
Definition: pointcloud_octomap_updater.h:154
occupancy_map_monitor::PointCloudOctomapUpdater::initialize
bool initialize() override
Definition: pointcloud_octomap_updater.cpp:129
occupancy_map_monitor::PointCloudOctomapUpdater::root_nh_
ros::NodeHandle root_nh_
Definition: pointcloud_octomap_updater.h:138
occupancy_map_monitor::PointCloudOctomapUpdater::padding_
double padding_
Definition: pointcloud_octomap_updater.h:149
occupancy_map_monitor::PointCloudOctomapUpdater::key_ray_
octomap::KeyRay key_ray_
Definition: pointcloud_octomap_updater.h:162
occupancy_map_monitor::PointCloudOctomapUpdater::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: pointcloud_octomap_updater.h:141
transform_listener.h
ShapeHandle
unsigned int ShapeHandle
occupancy_map_monitor::PointCloudOctomapUpdater::getShapeTransform
bool getShapeTransform(ShapeHandle h, Eigen::Isometry3d &transform) const
Definition: pointcloud_octomap_updater.cpp:198
occupancy_map_monitor::PointCloudOctomapUpdater::max_range_
double max_range_
Definition: pointcloud_octomap_updater.h:150
occupancy_map_monitor::PointCloudOctomapUpdater::PointCloudOctomapUpdater
PointCloudOctomapUpdater()
Definition: pointcloud_octomap_updater.cpp:83
ros::Time
occupancy_map_monitor::PointCloudOctomapUpdater::last_update_time_
ros::Time last_update_time_
Definition: pointcloud_octomap_updater.h:144
occupancy_map_monitor::PointCloudOctomapUpdater::stop
void stop() override
Definition: pointcloud_octomap_updater.cpp:175
occupancy_map_monitor::PointCloudOctomapUpdater::filtered_cloud_topic_
std::string filtered_cloud_topic_
Definition: pointcloud_octomap_updater.h:153
occupancy_map_monitor::PointCloudOctomapUpdater::max_update_rate_
double max_update_rate_
Definition: pointcloud_octomap_updater.h:152
occupancy_map_monitor
occupancy_map_monitor::PointCloudOctomapUpdater::setParams
bool setParams(XmlRpc::XmlRpcValue &params) override
Definition: pointcloud_octomap_updater.cpp:101
occupancy_map_monitor::PointCloudOctomapUpdater::point_cloud_filter_
tf2_ros::MessageFilter< sensor_msgs::PointCloud2 > * point_cloud_filter_
Definition: pointcloud_octomap_updater.h:158
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
occupancy_map_monitor::PointCloudOctomapUpdater::updateMask
virtual void updateMask(const sensor_msgs::PointCloud2 &cloud, const Eigen::Vector3d &sensor_origin, std::vector< int > &mask)
Definition: pointcloud_octomap_updater.cpp:208
occupancy_map_monitor::PointCloudOctomapUpdater::point_subsample_
unsigned int point_subsample_
Definition: pointcloud_octomap_updater.h:151
shape_mask.h
occupancy_map_monitor::PointCloudOctomapUpdater::cloudMsgCallback
void cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
Definition: pointcloud_octomap_updater.cpp:213
XmlRpc::XmlRpcValue
ros::NodeHandle
occupancy_map_updater.h


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon May 27 2024 02:27:57