Skip to content

Commit

Permalink
Use ros::Time instances instead of double when available.
Browse files Browse the repository at this point in the history
  • Loading branch information
@StefanGlaser
StefanGlaser committed Dec 3, 2017
1 parent aa9965d commit 55d90c4
Show file tree
Hide file tree
Showing 4 changed files with 101 additions and 105 deletions.
8 changes: 4 additions & 4 deletions include/loam_velodyne/LaserMapping.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,10 +190,10 @@ class LaserMapping {
std::vector<size_t> _laserCloudValidInd;
std::vector<size_t> _laserCloudSurroundInd;

double _timeLaserCloudCornerLast; ///< time of current last corner cloud
double _timeLaserCloudSurfLast; ///< time of current last surface cloud
double _timeLaserCloudFullRes; ///< time of current full resolution cloud
double _timeLaserOdometry; ///< time of current laser odometry
ros::Time _timeLaserCloudCornerLast; ///< time of current last corner cloud
ros::Time _timeLaserCloudSurfLast; ///< time of current last surface cloud
ros::Time _timeLaserCloudFullRes; ///< time of current full resolution cloud
ros::Time _timeLaserOdometry; ///< time of current laser odometry

bool _newLaserCloudCornerLast; ///< flag if a new last corner cloud has been received
bool _newLaserCloudSurfLast; ///< flag if a new last surface cloud has been received
Expand Down
23 changes: 10 additions & 13 deletions include/loam_velodyne/LaserOdometry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,22 +151,22 @@ class LaserOdometry {
pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsFlat; ///< flat surface points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsLessFlat; ///< less flat surface points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloud; ///< full resolution cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr _imuTrans; ///< IMU transformation information

pcl::PointCloud<pcl::PointXYZI>::Ptr _lastCornerCloud; ///< last corner points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _lastSurfaceCloud; ///< last surface points cloud

pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudOri; ///< point selection
pcl::PointCloud<pcl::PointXYZI>::Ptr _coeffSel; ///< point selection coefficients

nanoflann::KdTreeFLANN<pcl::PointXYZI> _lastCornerKDTree; ///< last corner cloud KD-tree
nanoflann::KdTreeFLANN<pcl::PointXYZI> _lastSurfaceKDTree; ///< last surface cloud KD-tree

double _timeCornerPointsSharp; ///< time of current sharp corner cloud
double _timeCornerPointsLessSharp; ///< time of current less sharp corner cloud
double _timeSurfPointsFlat; ///< time of current flat surface cloud
double _timeSurfPointsLessFlat; ///< time of current less flat surface cloud
double _timeLaserCloudFullRes; ///< time of current full resolution cloud
double _timeImuTrans; ///< time of current IMU transformation information
ros::Time _timeCornerPointsSharp; ///< time of current sharp corner cloud
ros::Time _timeCornerPointsLessSharp; ///< time of current less sharp corner cloud
ros::Time _timeSurfPointsFlat; ///< time of current flat surface cloud
ros::Time _timeSurfPointsLessFlat; ///< time of current less flat surface cloud
ros::Time _timeLaserCloudFullRes; ///< time of current full resolution cloud
ros::Time _timeImuTrans; ///< time of current IMU transformation information

bool _newCornerPointsSharp; ///< flag if a new sharp corner cloud has been received
bool _newCornerPointsLessSharp; ///< flag if a new less sharp corner cloud has been received
Expand All @@ -175,12 +175,6 @@ class LaserOdometry {
bool _newLaserCloudFullRes; ///< flag if a new full resolution cloud has been received
bool _newImuTrans; ///< flag if a new IMU transformation information cloud has been received

nav_msgs::Odometry _laserOdometry; ///< laser odometry message
tf::StampedTransform _laserOdometryTrans; ///< laser odometry transformation

size_t _lastCornerCloudSize; ///< size of the last corner cloud
size_t _lastSurfaceCloudSize; ///< size of the last surface cloud

std::vector<int> _pointSearchCornerInd1; ///< first corner point search index buffer
std::vector<int> _pointSearchCornerInd2; ///< second corner point search index buffer

Expand All @@ -197,6 +191,9 @@ class LaserOdometry {
Vector3 _imuShiftFromStart;
Vector3 _imuVeloFromStart;

nav_msgs::Odometry _laserOdometryMsg; ///< laser odometry message
tf::StampedTransform _laserOdometryTrans; ///< laser odometry transformation

ros::Publisher _pubLaserCloudCornerLast; ///< last corner cloud message publisher
ros::Publisher _pubLaserCloudSurfLast; ///< last surface cloud message publisher
ros::Publisher _pubLaserCloudFullRes; ///< full resolution cloud message publisher
Expand Down
30 changes: 14 additions & 16 deletions src/lib/LaserMapping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,17 +207,17 @@ void LaserMapping::transformUpdate()
if (_imuHistory.size() > 0) {
size_t imuIdx = 0;

while (imuIdx < _imuHistory.size() - 1 && _timeLaserOdometry + _scanPeriod > _imuHistory[imuIdx].stamp.toSec()) {
while (imuIdx < _imuHistory.size() - 1 && (_timeLaserOdometry - _imuHistory[imuIdx].stamp).toSec() + _scanPeriod > 0) {
imuIdx++;
}

IMUState2 imuCur;

if (imuIdx == 0 || _timeLaserOdometry + _scanPeriod > _imuHistory[imuIdx].stamp.toSec()) {
if (imuIdx == 0 || (_timeLaserOdometry - _imuHistory[imuIdx].stamp).toSec() + _scanPeriod > 0) {
// scan time newer then newest or older than oldest IMU message
imuCur = _imuHistory[imuIdx];
} else {
float ratio = (_imuHistory[imuIdx].stamp.toSec() - _timeLaserOdometry - _scanPeriod)
float ratio = ((_imuHistory[imuIdx].stamp - _timeLaserOdometry).toSec() - _scanPeriod)
/ (_imuHistory[imuIdx].stamp - _imuHistory[imuIdx - 1].stamp).toSec();

IMUState2::interpolate(_imuHistory[imuIdx], _imuHistory[imuIdx - 1], ratio, imuCur);
Expand Down Expand Up @@ -263,7 +263,7 @@ void LaserMapping::pointAssociateTobeMapped(const pcl::PointXYZI& pi, pcl::Point

void LaserMapping::laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg)
{
_timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp.toSec();
_timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp;

_laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerPointsLastMsg, *_laserCloudCornerLast);
Expand All @@ -275,7 +275,7 @@ void LaserMapping::laserCloudCornerLastHandler(const sensor_msgs::PointCloud2Con

void LaserMapping::laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg)
{
_timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp.toSec();
_timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp;

_laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfacePointsLastMsg, *_laserCloudSurfLast);
Expand All @@ -287,7 +287,7 @@ void LaserMapping::laserCloudSurfLastHandler(const sensor_msgs::PointCloud2Const

void LaserMapping::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg)
{
_timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp.toSec();
_timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp;

_laserCloudFullRes->clear();
pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloudFullRes);
Expand All @@ -299,7 +299,7 @@ void LaserMapping::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstP

void LaserMapping::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
_timeLaserOdometry = laserOdometry->header.stamp.toSec();
_timeLaserOdometry = laserOdometry->header.stamp;

double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
Expand Down Expand Up @@ -367,9 +367,9 @@ bool LaserMapping::hasNewData()
{
return _newLaserCloudCornerLast && _newLaserCloudSurfLast &&
_newLaserCloudFullRes && _newLaserOdometry &&
fabs(_timeLaserCloudCornerLast - _timeLaserOdometry) < 0.005 &&
fabs(_timeLaserCloudSurfLast - _timeLaserOdometry) < 0.005 &&
fabs(_timeLaserCloudFullRes - _timeLaserOdometry) < 0.005;
fabs((_timeLaserCloudCornerLast - _timeLaserOdometry).toSec()) < 0.005 &&
fabs((_timeLaserCloudSurfLast - _timeLaserOdometry).toSec()) < 0.005 &&
fabs((_timeLaserCloudFullRes - _timeLaserOdometry).toSec()) < 0.005;
}


Expand Down Expand Up @@ -1037,8 +1037,6 @@ void LaserMapping::optimizeTransformTobeMapped()

void LaserMapping::publishResult()
{
ros::Time odometryTime = ros::Time().fromSec(_timeLaserOdometry);

// publish new map cloud according to the input output ratio
_mapFrameCount++;
if (_mapFrameCount >= _mapFrameNum) {
Expand All @@ -1059,7 +1057,7 @@ void LaserMapping::publishResult()
_downSizeFilterCorner.filter(*_laserCloudSurroundDS);

// publish new map cloud
publishCloudMsg(_pubLaserCloudSurround, *_laserCloudSurroundDS, odometryTime, "/camera_init");
publishCloudMsg(_pubLaserCloudSurround, *_laserCloudSurroundDS, _timeLaserOdometry, "/camera_init");
}


Expand All @@ -1070,7 +1068,7 @@ void LaserMapping::publishResult()
}

// publish transformed full resolution input cloud
publishCloudMsg(_pubLaserCloudFullRes, *_laserCloudFullRes, odometryTime, "/camera_init");
publishCloudMsg(_pubLaserCloudFullRes, *_laserCloudFullRes, _timeLaserOdometry, "/camera_init");


// publish odometry after mapped transformations
Expand All @@ -1079,7 +1077,7 @@ void LaserMapping::publishResult()
-_transformAftMapped.rot_x.rad(),
-_transformAftMapped.rot_y.rad());

_odomAftMapped.header.stamp = odometryTime;
_odomAftMapped.header.stamp = _timeLaserOdometry;
_odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
_odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
_odomAftMapped.pose.pose.orientation.z = geoQuat.x;
Expand All @@ -1095,7 +1093,7 @@ void LaserMapping::publishResult()
_odomAftMapped.twist.twist.linear.z = _transformBefMapped.pos.z();
_pubOdomAftMapped.publish(_odomAftMapped);

_aftMappedTrans.stamp_ = odometryTime;
_aftMappedTrans.stamp_ = _timeLaserOdometry;
_aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
_aftMappedTrans.setOrigin(tf::Vector3(_transformAftMapped.pos.x(),
_transformAftMapped.pos.y(),
Expand Down
Loading

0 comments on commit 55d90c4

Please sign in to comment.