Adding support for PointCloud2 to the laser projector, removing a bunch of deprecated functions, and fixing a ton of warnings

git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35237 eb33c2ac-9c88-4c90-87e0-44a10359b0c3

src/laser_geometry.cpp

@@ -37,10 +37,10 @@ namespace laser_geometry
     LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, 
                                    bool preservative, int mask)
   {
-    boost::numeric::ublas::matrix<double> ranges(2, scan_in.get_ranges_size());
+    boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
 
     // Fill the ranges matrix
-    for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
+    for (unsigned int index = 0; index < scan_in.ranges.size(); index++)
       {
         ranges(0,index) = (double) scan_in.ranges[index];
         ranges(1,index) = (double) scan_in.ranges[index];
@@ -49,53 +49,53 @@ namespace laser_geometry
 
     //Do the projection
     //    NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
-    boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.get_ranges_size()));
+    boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size()));
 
     //Stuff the output cloud
     cloud_out.header = scan_in.header;
-    cloud_out.set_points_size (scan_in.get_ranges_size ());
+    cloud_out.points.resize (scan_in.ranges.size());
 
     // Define 4 indices in the channel array for each possible value type
     int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
 
-    cloud_out.set_channels_size(0);    
+    cloud_out.channels.resize(0);    
 
     // Check if the intensity bit is set
-    if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0)
+    if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size + 1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size + 1);
       cloud_out.channels[0].name = "intensities";
-      cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
+      cloud_out.channels[0].values.resize (scan_in.intensities.size());
       idx_intensity = 0;
     }
     
     // Check if the index bit is set
     if (mask & channel_option::Index)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size +1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size +1);
       cloud_out.channels[chan_size].name = "index";
-      cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
+      cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
       idx_index = chan_size;
     }
 
     // Check if the distance bit is set
     if (mask & channel_option::Distance)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size + 1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size + 1);
       cloud_out.channels[chan_size].name = "distances";
-      cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
+      cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
       idx_distance = chan_size;
     }
 
     if (mask & channel_option::Timestamp)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size + 1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size + 1);
       cloud_out.channels[chan_size].name = "stamps";
-      cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
+      cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
       idx_timestamp = chan_size;
     }
 
@@ -105,7 +105,7 @@ namespace laser_geometry
       range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); 
     
     unsigned int count = 0;
-    for (unsigned int index = 0; index< scan_in.get_ranges_size(); index++)
+    for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
     {
       if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= scan_in.range_min))) //if valid or preservative
       {
@@ -129,7 +129,7 @@ namespace laser_geometry
           cloud_out.channels[idx_distance].values[count] = ranges (0, index);
 
         // Save intensities channel
-        if (scan_in.get_intensities_size() >= index)
+        if (scan_in.intensities.size() >= index)
         { /// \todo optimize and catch length difference better
           if (idx_intensity != -1)
             cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
@@ -145,9 +145,9 @@ namespace laser_geometry
    
 
     //downsize if necessary
-    cloud_out.set_points_size (count);
-    for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++)
-      cloud_out.channels[d].set_values_size(count);
+    cloud_out.points.resize (count);
+    for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
+      cloud_out.channels[d].values.resize(count);
   };
 
 const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length)
@@ -189,54 +189,54 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
 
   void
     LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in,
-                                                     tf::Transformer& tf, int mask, bool preservative)
+                                                     tf::Transformer& tf, double range_cutoff, int mask, bool preservative)
   {
     cloud_out.header = scan_in.header;
     cloud_out.header.frame_id = target_frame;
-    cloud_out.set_points_size (scan_in.get_ranges_size());
+    cloud_out.points.resize (scan_in.ranges.size());
     
     // Define 4 indices in the channel array for each possible value type
     int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
 
 
-    cloud_out.set_channels_size(0);
+    cloud_out.channels.resize(0);
 
     // Check if the intensity bit is set
-    if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0)
+    if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size + 1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size + 1);
       cloud_out.channels[0].name = "intensities";
-      cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
+      cloud_out.channels[0].values.resize (scan_in.intensities.size());
       idx_intensity = 0;
     }
     
     // Check if the index bit is set
     if (mask & channel_option::Index)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size + 1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size + 1);
       cloud_out.channels[chan_size].name = "index";
-      cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
+      cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
       idx_index = chan_size;
     }
 
     // Check if the distance bit is set
     if (mask & channel_option::Distance)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size + 1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size + 1);
       cloud_out.channels[chan_size].name = "distances";
-      cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
+      cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
       idx_distance = chan_size;
     }
 
     if (mask & channel_option::Timestamp)
     {
-      int chan_size = cloud_out.get_channels_size ();
-      cloud_out.set_channels_size (chan_size + 1);
+      int chan_size = cloud_out.channels.size();
+      cloud_out.channels.resize (chan_size + 1);
       cloud_out.channels[chan_size].name = "stamps";
-      cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
+      cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
       idx_timestamp = chan_size;
     }
 
@@ -252,7 +252,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
 
     // Extract transforms for the beginning and end of the laser scan
     ros::Time start_time = scan_in.header.stamp ;
-    ros::Time end_time   = scan_in.header.stamp + ros::Duration().fromSec(scan_in.get_ranges_size()*scan_in.time_increment) ;
+    ros::Time end_time   = scan_in.header.stamp + ros::Duration().fromSec(scan_in.ranges.size()*scan_in.time_increment) ;
 
     tf::StampedTransform start_transform ;
     tf::StampedTransform end_transform ;
@@ -261,10 +261,15 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
     tf.lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ;
     tf.lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ;
 
+    if (range_cutoff < 0)
+      range_cutoff = scan_in.range_max;
+    else
+      range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); 
+
     unsigned int count = 0;  
-    for (unsigned int i = 0; i < scan_in.get_ranges_size(); i++)
+    for (unsigned int i = 0; i < scan_in.ranges.size(); i++)
     {
-      if (preservative || (scan_in.ranges[i] < scan_in.range_max && scan_in.ranges[i] > scan_in.range_min)) //only when valid
+      if (preservative || (scan_in.ranges[i] < range_cutoff && scan_in.ranges[i] > scan_in.range_min)) //only when valid
       {
         // Looking up transforms in tree is too expensive. Need more optimized way
         /*
@@ -275,7 +280,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
            */
 
         // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
-        btScalar ratio = i / ( (double) scan_in.get_ranges_size() - 1.0) ;
+        btScalar ratio = i / ( (double) scan_in.ranges.size() - 1.0) ;
 
         //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
 
@@ -309,7 +314,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
         if (idx_distance != -1)
           cloud_out.channels[idx_distance].values[count] = scan_in.ranges[i];
 
-        if (scan_in.get_intensities_size() >= i)
+        if (scan_in.intensities.size() >= i)
         { /// \todo optimize and catch length difference better
           if (idx_intensity != -1)
             cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[i];
@@ -323,9 +328,323 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
 
     }
     //downsize if necessary
-    cloud_out.set_points_size (count);
-    for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++)
-      cloud_out.channels[d].set_values_size (count);
+    cloud_out.points.resize (count);
+    for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
+      cloud_out.channels[d].values.resize (count);
+  }
+
+  void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, 
+                                      sensor_msgs::PointCloud2 &cloud_out,
+                                      Eigen3::ArrayXXd &co_sine_map,
+                                      double range_cutoff,
+                                      int channel_options)
+  {
+    size_t n_pts = scan_in.ranges.size ();
+
+    Eigen3::ArrayXXd ranges (n_pts, 2), output (n_pts, 2);
+
+    // Get the ranges into Eigen format
+    for (size_t i = 0; i < n_pts; ++i)
+    {
+      ranges (i, 0) = (double) scan_in.ranges[i];
+      ranges (i, 1) = (double) scan_in.ranges[i];
+    }
+
+    // Check if we have a precomputed co_sine_map
+    if (co_sine_map.rows () != (int)n_pts)
+    {
+      ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one.");
+      co_sine_map = Eigen3::ArrayXXd (n_pts, 2);
+      // Spherical->Cartesian projection
+      for (size_t i = 0; i < n_pts; ++i)
+      {
+        co_sine_map (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment);
+        co_sine_map (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment);
+      }
+    }
+
+    output = ranges * co_sine_map;
+
+    // Set the output cloud accordingly
+    cloud_out.header = scan_in.header;
+    cloud_out.height = 1;
+    cloud_out.width  = scan_in.ranges.size ();
+    cloud_out.fields.resize (3);
+    cloud_out.fields[0].name = "x";
+    cloud_out.fields[1].name = "y";
+    cloud_out.fields[2].name = "z";
+
+    // Define 4 indices in the channel array for each possible value type
+    int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1;
+
+    //now, we need to check what fields we need to store
+    int offset = 0;
+    if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0)
+    {
+      int field_size = cloud_out.fields.size();
+      cloud_out.fields.resize(field_size + 1);
+      cloud_out.fields[field_size].name = "intensity";
+      cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
+      cloud_out.fields[field_size].offset = offset;
+      offset += 4;
+      idx_intensity = field_size;
+    }
+
+    if ((channel_options & channel_option::Index))
+    {
+      int field_size = cloud_out.fields.size();
+      cloud_out.fields.resize(field_size + 1);
+      cloud_out.fields[field_size].name = "index";
+      cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32;
+      cloud_out.fields[field_size].offset = offset;
+      offset += 4;
+      idx_index = field_size;
+    }
+
+    if ((channel_options & channel_option::Distance))
+    {
+      int field_size = cloud_out.fields.size();
+      cloud_out.fields.resize(field_size + 1);
+      cloud_out.fields[field_size].name = "distances";
+      cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
+      cloud_out.fields[field_size].offset = offset;
+      offset += 4;
+      idx_distance = field_size;
+    }
+
+    if ((channel_options & channel_option::Timestamp))
+    {
+      int field_size = cloud_out.fields.size();
+      cloud_out.fields.resize(field_size + 1);
+      cloud_out.fields[field_size].name = "stamps";
+      cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
+      cloud_out.fields[field_size].offset = offset;
+      offset += 4;
+      idx_timestamp = field_size;
+    }
+
+    if ((channel_options & channel_option::Viewpoint))
+    {
+      int field_size = cloud_out.fields.size();
+      cloud_out.fields.resize(field_size + 3);
+
+      cloud_out.fields[field_size].name = "vp_x";
+      cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
+      cloud_out.fields[field_size].offset = offset;
+      offset += 4;
+
+      cloud_out.fields[field_size + 1].name = "vp_y";
+      cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32;
+      cloud_out.fields[field_size + 1].offset = offset;
+      offset += 4;
+
+      cloud_out.fields[field_size + 2].name = "vp_z";
+      cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32;
+      cloud_out.fields[field_size + 2].offset = offset;
+      offset += 4;
+
+      idx_vpx = field_size;
+      idx_vpy = field_size + 1;
+      idx_vpz = field_size + 2;
+    }
+
+    cloud_out.point_step = offset;
+    cloud_out.row_step   = cloud_out.point_step * cloud_out.width;
+    cloud_out.data.resize (cloud_out.row_step   * cloud_out.height);
+    cloud_out.is_dense = true;
+
+    //TODO: Find out why this was needed
+    //float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+    unsigned int count = 0;
+    for (size_t i = 0; i < n_pts; ++i)
+    {
+      if (range_cutoff < 0)
+        range_cutoff = scan_in.range_max;
+      else
+        range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); 
+
+      //check to see if we want to keep the point
+      if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
+      {
+
+        float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
+
+        // Copy XYZ
+        pstep[0] = output (i, 0);
+        pstep[1] = output (i, 1);
+        pstep[2] = 0;
+
+        // Copy intensity
+        if(idx_intensity != -1)
+          pstep[idx_intensity] = scan_in.intensities[i];
+
+        //Copy index
+        if(idx_index != -1)
+          pstep[idx_index] = i;
+
+        // Copy distance
+        if(idx_distance != -1)
+          pstep[idx_distance] = scan_in.ranges[i];
+
+        // Copy timestamp
+        if(idx_timestamp != -1)
+          pstep[idx_timestamp] = i * scan_in.time_increment;
+
+        // Copy viewpoint (0, 0, 0)
+        if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1)
+        {
+          pstep[idx_vpx] = 0;
+          pstep[idx_vpy] = 0;
+          pstep[idx_vpz] = 0;
+        }
+
+        //make sure to increment count
+        ++count;
+      }
+
+      /* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values
+       * why can't you just leave them out?
+       *
+      // Invalid measurement?
+      if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
+      {
+        if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE)
+        {
+          for (size_t s = 0; s < cloud_out.fields.size (); ++s)
+            pstep[s] = bad_point;
+        }
+        else
+        {
+          // Kind of nasty thing:
+          //   We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid.
+          //   Since we still might need the x value we store it in the distance field
+          pstep[0] = bad_point;           // X -> NAN to mark a bad point
+          pstep[1] = co_sine_map (i, 1);  // Y
+          pstep[2] = 0;                   // Z
+
+          if (store_intensity)
+          {
+            pstep[3] = bad_point;           // Intensity -> NAN to mark a bad point
+            pstep[4] = co_sine_map (i, 0);  // Distance -> Misused to store the originnal X
+          }
+          else
+            pstep[3] = co_sine_map (i, 0);  // Distance -> Misused to store the originnal X
+        }
+      }
+      */
+    }
+
+    //resize if necessary
+    cloud_out.width = count;
+    cloud_out.row_step   = cloud_out.point_step * cloud_out.width;
+    cloud_out.data.resize (cloud_out.row_step   * cloud_out.height);
+  }
+
+  void LaserProjection::transformLaserScanToPointCloud2_ (const std::string &target_frame, 
+                                                        const sensor_msgs::LaserScan &scan_in,
+                                                        sensor_msgs::PointCloud2 &cloud_out, 
+                                                        tf::Transformer &tf, 
+                                                        Eigen3::ArrayXXd &co_sine_map,
+                                                        double range_cutoff,
+                                                        int channel_options)
+  {
+    //we'll enforce that we get index values for the laser scan so that we
+    //ensure that we use the correct timestamps
+    channel_options |= channel_option::Index;
+
+    projectLaser_(scan_in, cloud_out, co_sine_map, -1.0, channel_options);
+
+    //we'll assume no associated viewpoint by default
+    bool has_viewpoint = false;
+    uint32_t vp_x_offset = 0;
+
+    //we need to find the offset of the intensity field in the point cloud
+    //we also know that the index field is guaranteed to exist since we 
+    //set the channel option above. To be really safe, it might be worth
+    //putting in a check at some point, but I'm just going to put in an
+    //assert for now
+    uint32_t index_offset = 0;
+    for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
+    {
+      if(cloud_out.fields[i].name == "index")
+      {
+        index_offset = cloud_out.fields[i].offset;
+      }
+
+      //we want to check if the cloud has a viewpoint associated with it
+      //checking vp_x should be sufficient since vp_x, vp_y, and vp_z all
+      //get put in together
+      if(cloud_out.fields[i].name == "vp_x")
+      {
+        has_viewpoint = true;
+        vp_x_offset = cloud_out.fields[i].offset;
+      }
+    }
+
+    ROS_ASSERT(index_offset > 0);
+
+    cloud_out.header.frame_id = target_frame;
+
+    // Extract transforms for the beginning and end of the laser scan
+    ros::Time start_time = scan_in.header.stamp;
+    ros::Time end_time   = scan_in.header.stamp + ros::Duration ().fromSec (scan_in.ranges.size () * scan_in.time_increment);
+
+    tf::StampedTransform start_transform, end_transform, cur_transform ;
+
+    tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform);
+    tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform);
+
+    double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0);
+
+    //we want to loop through all the points in the cloud
+    for(size_t i = 0; i < cloud_out.width; ++i)
+    {
+      // Apply the transform to the current point
+      float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
+
+      //find the index of the point
+      uint32_t pt_index;
+      memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
+      
+      // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
+      btScalar ratio = pt_index * ranges_norm;
+
+      //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
+      // Interpolate translation
+      btVector3 v (0, 0, 0);
+      v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio);
+      cur_transform.setOrigin (v);
+
+      // Interpolate rotation
+      btQuaternion q1, q2;
+      start_transform.getBasis ().getRotation (q1);
+      end_transform.getBasis ().getRotation (q2);
+
+      // Compute the slerp-ed rotation
+      cur_transform.setRotation (slerp (q1, q2 , ratio));
+
+      btVector3 point_in (pstep[0], pstep[1], pstep[2]);
+      btVector3 point_out = cur_transform * point_in;
+
+      // Copy transformed point into cloud
+      pstep[0] = point_out.x ();
+      pstep[1] = point_out.y ();
+      pstep[2] = point_out.z ();
+
+      // Convert the viewpoint as well
+      if(has_viewpoint)
+      {
+        float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
+        point_in = btVector3 (vpstep[0], vpstep[1], vpstep[2]);
+        point_out = cur_transform * point_in;
+
+        // Copy transformed point into cloud
+        vpstep[0] = point_out.x ();
+        vpstep[1] = point_out.y ();
+        vpstep[2] = point_out.z ();
+      }
+    }
   }
 
 

src/laser_scan_geometry.cpp

@@ -255,14 +255,22 @@ void
 }
 
 void
-  laser_scan_geometry::LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out)
+  laser_scan_geometry::LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, 
+                                                      sensor_msgs::PointCloud2 &cloud_out,
+                                                      double range_cutoff = -1.0,
+                                                      int channel_options = channel_option::Default)
 {
   boost::mutex::scoped_lock guv_lock (guv_mutex_);
   laser_scan_geometry::projectLaser (scan_in, cloud_out, co_sine_map_);
 }
 
 void
-  laser_scan_geometry::LaserProjection::transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, tf::Transformer &tf, sensor_msgs::PointCloud2 &cloud_out)
+  laser_scan_geometry::LaserProjection::transformLaserScanToPointCloud (const std::string &target_frame, 
+                                                                        const sensor_msgs::LaserScan &scan_in, 
+                                                                        sensor_msgs::PointCloud2 &cloud_out,
+                                                                        tf::Transformer &tf,
+                                                                        double range_cutoff,
+                                                                        int channel_options = channel_option::Default)
 {
   laser_scan_geometry::transformLaserScanToPointCloud (target_frame, scan_in, tf, cloud_out, co_sine_map_);
 }