Fix mistake in end_time calculation for scan transformation

This commit is contained in:
Andreas Wachaja 2013-12-20 17:13:34 +01:00
parent 6fc2057f7d
commit de644f402e

View File

@ -214,7 +214,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
// Extract transforms for the beginning and end of the laser scan // Extract transforms for the beginning and end of the laser scan
ros::Time start_time = scan_in.header.stamp ; 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 start_transform ;
tf::StampedTransform end_transform ; tf::StampedTransform end_transform ;
@ -550,7 +550,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
// Extract transforms for the beginning and end of the laser scan // Extract transforms for the beginning and end of the laser scan
ros::Time start_time = scan_in.header.stamp; 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 ; tf::StampedTransform start_transform, end_transform, cur_transform ;