laser_scan_assembler.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 the 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 
37 #include "sensor_msgs/LaserScan.h"
39 #include "filters/filter_chain.h"
40 
41 using namespace laser_geometry;
42 using namespace std ;
43 
44 namespace laser_assembler
45 {
46 
50 class LaserScanAssembler : public BaseAssembler<sensor_msgs::LaserScan>
51 {
52 public:
53  LaserScanAssembler() : BaseAssembler<sensor_msgs::LaserScan>("max_scans"), filter_chain_("sensor_msgs::LaserScan")
54  {
55  // ***** Set Laser Projection Method *****
56  private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true);
57 
58  // configure the filter chain from the parameter server
59  filter_chain_.configure("filters", private_ns_);
60 
61  // Have different callbacks, depending on whether or not we want to ignore laser skews.
62  if (ignore_laser_skew_)
63  start("scan");
64  else
65  {
66  start();
67  skew_scan_sub_ = n_.subscribe("scan", 10, &LaserScanAssembler::scanCallback, this);
68  }
69  }
70 
72  {
73 
74  }
75 
76  unsigned int GetPointsInScan(const sensor_msgs::LaserScan& scan)
77  {
78  return (scan.ranges.size ());
79  }
80 
81  void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out)
82  {
83  // apply filters on laser scan
84  filter_chain_.update (scan_in, scan_filtered_);
85 
86  // convert laser scan to point cloud
87  if (ignore_laser_skew_) // Do it the fast (approximate) way
88  {
89  projector_.projectLaser(scan_filtered_, cloud_out);
90  if (cloud_out.header.frame_id != fixed_frame_id)
91  tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out);
92  }
93  else // Do it the slower (more accurate) way
94  {
99  projector_.transformLaserScanToPointCloud (fixed_frame_id, scan_filtered_, cloud_out, *tf_, mask);
100  }
101  return;
102  }
103 
104  void scanCallback(const sensor_msgs::LaserScanConstPtr& laser_scan)
105  {
106  if (!ignore_laser_skew_)
107  {
108  ros::Duration cur_tolerance = ros::Duration(laser_scan->time_increment * laser_scan->ranges.size());
109  if (cur_tolerance > max_tolerance_)
110  {
111  ROS_DEBUG("Upping tf tolerance from [%.4fs] to [%.4fs]", max_tolerance_.toSec(), cur_tolerance.toSec());
112  assert(tf_filter_);
113  tf_filter_->setTolerance(cur_tolerance);
114  max_tolerance_ = cur_tolerance;
115  }
116  tf_filter_->add(laser_scan);
117  }
118  }
119 
120 private:
123 
125  ros::Duration max_tolerance_; // The longest tolerance we've needed on a scan so far
126 
128  mutable sensor_msgs::LaserScan scan_filtered_;
129 
130 };
131 
132 }
133 
134 using namespace laser_assembler ;
135 
136 int main(int argc, char **argv)
137 {
138  ros::init(argc, argv, "laser_scan_assembler");
139  LaserScanAssembler pc_assembler;
140  ros::spin();
141 
142  return 0;
143 }
filters::FilterChain< sensor_msgs::LaserScan > filter_chain_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
laser_geometry::LaserProjection projector_
int main(int argc, char **argv)
ROSCPP_DECL void spin(Spinner &spinner)
Maintains a history of point clouds and generates an aggregate point cloud upon request.
Maintains a history of laser scans and generates a point cloud upon request.
void ConvertToCloud(const string &fixed_frame_id, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out)
unsigned int GetPointsInScan(const sensor_msgs::LaserScan &scan)
Returns the number of points in the current scan.
#define ROS_DEBUG(...)
void scanCallback(const sensor_msgs::LaserScanConstPtr &laser_scan)


laser_assembler
Author(s): Vijay Pradeep
autogenerated on Thu Jun 27 2019 19:47:56