diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 2e2befe..ea27fd6 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -214,7 +214,7 @@ const boost::numeric::ublas::matrix& 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.ranges.size()*scan_in.time_increment) ; + ros::Time end_time = scan_in.header.stamp + ros::Duration().fromSec( (scan_in.ranges.size()-1) * scan_in.time_increment); tf::StampedTransform start_transform ; tf::StampedTransform end_transform ; @@ -550,7 +550,7 @@ const boost::numeric::ublas::matrix& 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.ranges.size () * scan_in.time_increment); + ros::Time end_time = scan_in.header.stamp + ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment); tf::StampedTransform start_transform, end_transform, cur_transform ;