47 const double& observation_keep_time, \
48 const double& expected_update_rate, \
49 const double& min_obstacle_height, \
50 const double& max_obstacle_height, \
51 const double& obstacle_range, \
53 const std::string& global_frame, \
54 const std::string& sensor_frame, \
55 const double& tf_tolerance, \
56 const double& min_d, \
57 const double& max_d, \
59 const double& vFOVPadding, \
61 const double& decay_acceleration, \
62 const bool& marking, \
63 const bool& clearing, \
64 const double& voxel_size, \
66 const int& voxel_min_points, \
67 const bool& enabled, \
68 const bool& clear_buffer_after_reading, \
71 _buffer(
tf), _observation_keep_time(observation_keep_time),
72 _expected_update_rate(expected_update_rate),_last_updated(
ros::Time::now()),
73 _global_frame(global_frame), _sensor_frame(sensor_frame),
74 _topic_name(topic_name), _min_obstacle_height(min_obstacle_height),
75 _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range),
76 _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d),
77 _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding),
78 _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration),
79 _marking(marking), _clearing(clearing), _voxel_size(voxel_size),
80 _filter(filter), _voxel_min_points(voxel_min_points),
81 _enabled(enabled), _clear_buffer_after_reading(clear_buffer_after_reading),
82 _model_type(model_type)
99 const std::string origin_frame = \
105 geometry_msgs::PoseStamped local_pose, global_pose;
106 local_pose.pose.position.x=0;
107 local_pose.pose.position.y=0;
108 local_pose.pose.position.z=0;
109 local_pose.pose.orientation.x=0;
110 local_pose.pose.orientation.y=0;
111 local_pose.pose.orientation.z=0;
112 local_pose.pose.orientation.w=1;
113 local_pose.header.stamp = cloud.header.stamp;
114 local_pose.header.frame_id = origin_frame;
148 pcl::PCLPointCloud2::Ptr cloud_pcl (
new pcl::PCLPointCloud2 ());
149 pcl::PCLPointCloud2::Ptr cloud_filtered (
new pcl::PCLPointCloud2 ());
156 pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
157 sor.setInputCloud (cloud_pcl);
158 sor.setFilterFieldName(
"z");
160 sor.setDownsampleAllData(
false);
164 sor.setMinimumPointsNumberPerVoxel(
static_cast<unsigned int>(
_voxel_min_points));
165 sor.filter(*cloud_filtered);
171 pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;
172 pass_through_filter.setInputCloud(cloud_pcl);
173 pass_through_filter.setKeepOrganized(
false);
174 pass_through_filter.setFilterFieldName(
"z");
175 pass_through_filter.setFilterLimits( \
177 pass_through_filter.filter(*cloud_filtered);
188 "TF Exception for sensor frame: %s, cloud frame: %s, %s", \
189 _sensor_frame.c_str(), cloud.header.frame_id.c_str(), ex.what());
199 std::vector<observation::MeasurementReading>& observations)
207 observations.push_back(*it);
264 const bool current = update_time.
toSec() <= _expected_update_rate.toSec();
268 "%s buffer updated in %.2fs, it should be updated every %.2fs.",
269 _topic_name.c_str(), update_time.
toSec(), _expected_update_rate.toSec());