@@ -84,7 +84,7 @@ void IpaPointCloudToLaserScanNodelet::onInit()
84
84
nh_ = getMTNodeHandle ();
85
85
}
86
86
87
- // Only queue one pointcloud per running thread
87
+ // Only queue one point cloud per running thread
88
88
if (concurrency_level > 0 )
89
89
{
90
90
input_queue_size_ = concurrency_level;
@@ -94,28 +94,28 @@ void IpaPointCloudToLaserScanNodelet::onInit()
94
94
input_queue_size_ = boost::thread::hardware_concurrency ();
95
95
}
96
96
97
- // if pointcloud target frame specified, we need to filter by transform availability
97
+ // if point cloud target frame specified, we need to filter by transform availability
98
98
if (!target_frame_.empty ())
99
99
{
100
100
tf2_.reset (new tf2_ros::Buffer ());
101
101
tf2_listener_.reset (new tf2_ros::TransformListener (*tf2_));
102
102
}
103
103
104
+ // set publisher and subscriber callback
104
105
pub_ = nh_.advertise <sensor_msgs::LaserScan>(" scan" , 10 );
105
- // set subscriber and callback for input cloud
106
106
sub_ = nh_.subscribe (" cloud_in" , input_queue_size_, &IpaPointCloudToLaserScanNodelet::cloudCb, this );
107
107
}
108
108
109
109
void IpaPointCloudToLaserScanNodelet::configure_filter ()
110
110
{
111
111
NODELET_DEBUG_STREAM (" configuring filter" );
112
- // Get filter related parameters
113
-
114
- private_nh_.param <bool >(" use_outlier_filter" , use_outlier_filter_, false );
115
112
113
+ // Get filter related parameters
116
114
double max_noise_cluster_distance;
117
115
double cluster_break_distance;
118
116
int max_noise_cluster_size;
117
+
118
+ private_nh_.param <bool >(" use_outlier_filter" , use_outlier_filter_, false );
119
119
private_nh_.param <double >(" max_noise_cluster_distance" , max_noise_cluster_distance, 2.0 );
120
120
private_nh_.param <double >(" cluster_break_distance" , cluster_break_distance, 0.3 );
121
121
private_nh_.param <int >(" max_noise_cluster_size" , max_noise_cluster_size, 5 );
@@ -126,7 +126,7 @@ void IpaPointCloudToLaserScanNodelet::configure_filter()
126
126
void IpaPointCloudToLaserScanNodelet::cloudCb (const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
127
127
{
128
128
ros::Time start_time = ros::Time::now ();
129
- NODELET_DEBUG_STREAM (" PC with timestamp from init " << cloud_msg->header .stamp .toSec () << " recevied with a delay of " << (start_time - cloud_msg->header .stamp ).toSec () << " " );
129
+ NODELET_DEBUG_STREAM (" point cloud with timestamp from init " << cloud_msg->header .stamp .toSec () << " recevied with a delay of " << (start_time - cloud_msg->header .stamp ).toSec () << " " );
130
130
131
131
// remove leading / on frame id in case present, which is not supported by tf2
132
132
// does not do anything if the problem dies not occur -> leave for compatibility
@@ -136,7 +136,7 @@ void IpaPointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2Cons
136
136
cloud_frame_id.erase (0 ,1 );
137
137
}
138
138
139
- // Get frame tranformation
139
+ // Get frame transformation
140
140
tf2::Transform T;
141
141
142
142
if ((!target_frame_.empty ()) && !(target_frame_ == cloud_frame_id))
@@ -185,11 +185,11 @@ void IpaPointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2Cons
185
185
}
186
186
else
187
187
{
188
- // Assign scan to almost max range since assign to max_range not allowed by assign oparator
188
+ // Assign scan to almost max range since assign to max_range not allowed by assign operator
189
189
output.ranges .assign (ranges_size, output.range_max - 0.0001 );
190
190
}
191
191
192
- // convert pointcloud to laserscan
192
+ // convert point cloud to laserscan
193
193
convert_pointcloud_to_laserscan (cloud_msg, output, T, range_min_);
194
194
195
195
if (use_outlier_filter_)
@@ -199,18 +199,18 @@ void IpaPointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2Cons
199
199
200
200
ros::Time end_time = ros::Time::now ();
201
201
ros::Duration dur = end_time-start_time;
202
- NODELET_DEBUG_STREAM (" Transform for PC took " << dur.toSec ());
202
+ NODELET_DEBUG_STREAM (" Transform for point cloud took " << dur.toSec ());
203
203
204
204
pub_.publish (output);
205
- NODELET_DEBUG_STREAM (" Transform and publisch for scan took " << dur.toSec ());
205
+ NODELET_DEBUG_STREAM (" Transform and publish for scan took " << dur.toSec ());
206
206
}
207
207
208
208
/* *
209
- * Function to project the pointcloud points within specified region to
209
+ * Function to project the point cloud points within specified region to
210
210
* the laser scan frame and fill out the laserscan message with the relevant ranges
211
211
* from the projection.
212
- * Theborders for the point selection is transformed into pointcloud frame in order
213
- * save time by avoiding unnessecairy point transformations
212
+ * The borders for the point selection is transformed into point cloud frame in order
213
+ * save time by avoiding unnecessary point transformations
214
214
*/
215
215
void IpaPointCloudToLaserScanNodelet::convert_pointcloud_to_laserscan (const sensor_msgs::PointCloud2ConstPtr &cloud,
216
216
sensor_msgs::LaserScan &output,
@@ -244,15 +244,15 @@ void IpaPointCloudToLaserScanNodelet::convert_pointcloud_to_laserscan(const sens
244
244
245
245
// Declare help variables
246
246
tf2::Vector3 P;
247
- double lambda_x, lambda_y;
248
247
tf2::Vector3 P_max;
249
248
tf2::Vector3 P_min;
249
+ double lambda_x, lambda_y;
250
250
double border_distance_sqared;
251
251
double range;
252
252
double angle;
253
253
int index;
254
254
255
- // Iterate through pointcloud
255
+ // Iterate through point cloud
256
256
for (sensor_msgs::PointCloud2ConstIterator<float >
257
257
iter_x (*cloud, " x" ), iter_y (*cloud, " y" ), iter_z (*cloud, " z" );
258
258
iter_x != iter_x.end ();
@@ -264,7 +264,7 @@ void IpaPointCloudToLaserScanNodelet::convert_pointcloud_to_laserscan(const sens
264
264
continue ;
265
265
}
266
266
267
- // get reflection point in hight limiting planes in order to check that point lies between borders(above or below is not clearly def):
267
+ // get reflection point in height limiting planes in order to check that point lies between borders(above or below is not clearly def):
268
268
P.setValue (*iter_x, *iter_y, *iter_z);
269
269
270
270
/* *
@@ -274,7 +274,7 @@ void IpaPointCloudToLaserScanNodelet::convert_pointcloud_to_laserscan(const sens
274
274
*
275
275
* double lambda_x_max = (P - A_max_o_frame).dot(ex_o_frame);
276
276
*
277
- * For now we assume tahat a common set of lambdas hold:
277
+ * For now we assume that a common set of lambdas hold:
278
278
*/
279
279
lambda_x = (P - A_target_o_frame).dot (ex_o_frame);
280
280
lambda_y = (P - A_target_o_frame).dot (ey_o_frame);
0 commit comments