diff --git a/config/params.yaml b/config/params.yaml
index d1563628a6f82c9ae90429872b369248d15f5a5a..5d5cfff1657a0f3ede66d90147c5a41563c8cbbb 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -6,6 +6,11 @@ lio_sam:
odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
+ # Frames
+ lidarFrame: "base_link"
+ odometryFrame: "odom"
+ mapFrame: "map"
+
# GPS Settings
useImuHeadingInitialization: true # if using GPS data, set to "true"
useGpsElevation: false # if GPS elevation is bad, set to "false"
diff --git a/include/utility.h b/include/utility.h
index 55a186f5bbcdbbc7b0358098d43e418a6125374a..fbff9eb21ac01d6c551360b22fccfc639a91d794 100644
--- a/include/utility.h
+++ b/include/utility.h
@@ -62,11 +62,17 @@ public:
std::string robot_id;
+ //Topics
string pointCloudTopic;
string imuTopic;
string odomTopic;
string gpsTopic;
+ //Frames
+ string lidarFrame;
+ string odometryFrame;
+ string mapFrame;
+
// GPS Settings
bool useImuHeadingInitialization;
bool useGpsElevation;
@@ -143,6 +149,10 @@ public:
nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
+ nh.param<std::string>("lio_sam/lidarFrame", lidarFrame, "base_link");
+ nh.param<std::string>("lio_sam/odometryFrame", odometryFrame, "odom");
+ nh.param<std::string>("lio_sam/mapFrame", mapFrame, "map");
+
nh.param<bool>("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);
nh.param<bool>("lio_sam/useGpsElevation", useGpsElevation, false);
nh.param<float>("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);
diff --git a/src/featureExtraction.cpp b/src/featureExtraction.cpp
index abed18b60cc21d7814a6b6063e3bf3899bfc378c..489b04e2b3749ce8f1f2c3c601b70ca6cc64e271 100644
--- a/src/featureExtraction.cpp
+++ b/src/featureExtraction.cpp
@@ -250,8 +250,8 @@ public:
// free cloud info memory
freeCloudInfoMemory();
// save newly extracted features
- cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, "base_link");
- cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, "base_link");
+ cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
+ cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
// publish to mapOptimization
pubLaserCloudInfo.publish(cloudInfo);
}
diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp
index a03d4a53ea9d7dc7c0ea317aa7a01f3e1099a860..aaf3ab7c4375ef3a256178989a5bd05e5d27ba09 100644
--- a/src/imageProjection.cpp
+++ b/src/imageProjection.cpp
@@ -573,7 +573,7 @@ public:
void publishClouds()
{
cloudInfo.header = cloudHeader;
- cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, "base_link");
+ cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}
};
diff --git a/src/imuPreintegration.cpp b/src/imuPreintegration.cpp
index 55f4d2883a97608f27386feed1bf77401db7aec5..26a5d753b6ee6e93bbe228a408b85cf7333dab21 100644
--- a/src/imuPreintegration.cpp
+++ b/src/imuPreintegration.cpp
@@ -337,7 +337,7 @@ public:
{
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
// publish static tf
- tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, thisImu.header.stamp, "map", "odom"));
+ tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, thisImu.header.stamp, mapFrame, odometryFrame));
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
@@ -359,7 +359,7 @@ public:
// publish odometry
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
- odometry.header.frame_id = "odom";
+ odometry.header.frame_id = odometryFrame;
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar
@@ -391,7 +391,7 @@ public:
last_path_time = imuTime;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = thisImu.header.stamp;
- pose_stamped.header.frame_id = "odom";
+ pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose = odometry.pose.pose;
imuPath.poses.push_back(pose_stamped);
while(!imuPath.poses.empty() && abs(imuPath.poses.front().header.stamp.toSec() - imuPath.poses.back().header.stamp.toSec()) > 3.0)
@@ -399,7 +399,7 @@ public:
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = thisImu.header.stamp;
- imuPath.header.frame_id = "odom";
+ imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
@@ -407,7 +407,7 @@ public:
// publish transformation
tf::Transform tCur;
tf::poseMsgToTF(odometry.pose.pose, tCur);
- tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, thisImu.header.stamp, "odom", "base_link");
+ tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, thisImu.header.stamp, odometryFrame, lidarFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);
}
};
@@ -424,4 +424,4 @@ int main(int argc, char** argv)
ros::spin();
return 0;
-}
\ No newline at end of file
+}
diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp
index ac21d47c1d6b283932323b0abd0303fca7875f3e..e6c69c3d146978e549edda607302557538c67948 100644
--- a/src/mapOptmization.cpp
+++ b/src/mapOptmization.cpp
@@ -424,7 +424,7 @@ public:
downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
- publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, "odom");
+ publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);
}
@@ -534,7 +534,7 @@ public:
downSizeFilterICP.filter(*cloud_temp);
*nearHistoryKeyFrameCloud = *cloud_temp;
// publish history near key frames
- publishCloud(&pubHistoryKeyFrames, nearHistoryKeyFrameCloud, timeLaserInfoStamp, "odom");
+ publishCloud(&pubHistoryKeyFrames, nearHistoryKeyFrameCloud, timeLaserInfoStamp, odometryFrame);
// Align clouds
icp.setInputSource(latestKeyFrameCloud);
@@ -550,7 +550,7 @@ public:
if (pubIcpKeyFrames.getNumSubscribers() != 0){
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*latestKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());
- publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, "odom");
+ publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
// Get pose transformation
@@ -1397,7 +1397,7 @@ public:
{
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
- pose_stamped.header.frame_id = "odom";
+ pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose.position.x = pose_in.x;
pose_stamped.pose.position.y = pose_in.y;
pose_stamped.pose.position.z = pose_in.z;
@@ -1415,7 +1415,7 @@ public:
// Publish odometry for ROS
nav_msgs::Odometry laserOdometryROS;
laserOdometryROS.header.stamp = timeLaserInfoStamp;
- laserOdometryROS.header.frame_id = "odom";
+ laserOdometryROS.header.frame_id = odometryFrame;
laserOdometryROS.child_frame_id = "odom_mapping";
laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
@@ -1430,9 +1430,9 @@ public:
if (cloudKeyPoses3D->points.empty())
return;
// publish key poses
- publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, "odom");
+ publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
// Publish surrounding key frames
- publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, "odom");
+ publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
// publish registered key frame
if (pubRecentKeyFrame.getNumSubscribers() != 0)
{
@@ -1440,7 +1440,7 @@ public:
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
*cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
- publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, "odom");
+ publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish registered high-res raw cloud
if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
@@ -1449,13 +1449,13 @@ public:
pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
- publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, "odom");
+ publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish path
if (pubPath.getNumSubscribers() != 0)
{
globalPath.header.stamp = timeLaserInfoStamp;
- globalPath.header.frame_id = "odom";
+ globalPath.header.frame_id = odometryFrame;
pubPath.publish(globalPath);
}
}
@@ -1479,4 +1479,4 @@ int main(int argc, char** argv)
visualizeMapThread.join();
return 0;
-}
\ No newline at end of file
+}