i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

added pub_loc_pose_only_when_localizing option (#980)

Browse Source 
Addressing https://github.com/introlab/rtabmap_ros/issues/979
This commit is contained in:
matlabbe
2023-06-06 13:40:07 -07:00
committed by GitHub
parent 1bf27fb2d9
commit 1a87f1598d
2 changed files with 33 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+1
View File
@@ -256,6 +256,7 @@ private:
rtabmap::Transform currentMetricGoal_; rtabmap::Transform currentMetricGoal_;
rtabmap::Transform lastPublishedMetricGoal_; rtabmap::Transform lastPublishedMetricGoal_;
bool latestNodeWasReached_; bool latestNodeWasReached_;
bool pubLocPoseOnlyWhenLocalizing_;
bool graphLatched_; bool graphLatched_;
rtabmap::ParametersMap parameters_; rtabmap::ParametersMap parameters_;
std::map<std::string, float> rtabmapROSStats_; std::map<std::string, float> rtabmapROSStats_;
rtabmap_slam/src/CoreWrapper.cpp
+32 -10
View File
@@ -89,6 +89,7 @@ CoreWrapper::CoreWrapper() :
lastPose_(Transform::getIdentity()), lastPose_(Transform::getIdentity()),
lastPoseIntermediate_(false), lastPoseIntermediate_(false),
latestNodeWasReached_(false), latestNodeWasReached_(false),
pubLocPoseOnlyWhenLocalizing_(false),
graphLatched_(false), graphLatched_(false),
frameId_("base_link"), frameId_("base_link"),
odomFrameId_(""), odomFrameId_(""),
@@ -192,6 +193,7 @@ void CoreWrapper::onInit()
pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_); pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
pnh.param("landmark_angular_variance", landmarkDefaultAngVariance_, landmarkDefaultAngVariance_); pnh.param("landmark_angular_variance", landmarkDefaultAngVariance_, landmarkDefaultAngVariance_);
pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_); pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_);
pnh.param("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_,pubLocPoseOnlyWhenLocalizing_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr); pnh.param("initial_pose", initialPoseStr, initialPoseStr);
@@ -244,6 +246,7 @@ void CoreWrapper::onInit()
NODELET_INFO("rtabmap: tf_delay = %f", tfDelay); NODELET_INFO("rtabmap: tf_delay = %f", tfDelay);
NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance); NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false");
NODELET_INFO("rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false");
bool subscribeStereo = false; bool subscribeStereo = false;
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
if(subscribeStereo) if(subscribeStereo)
@@ -2043,17 +2046,36 @@ void CoreWrapper::process(
} }
else else
{ {
if(localizationPosePub_.getNumSubscribers() && if(localizationPosePub_.getNumSubscribers())
!rtabmap_.getStatistics().localizationCovariance().empty())
{ {
geometry_msgs::PoseWithCovarianceStamped poseMsg; bool localized = rtabmap_.getStatistics().loopClosureId()!=0 ||
poseMsg.header.frame_id = mapFrameId_; rtabmap_.getStatistics().proximityDetectionId()!=0 ||
poseMsg.header.stamp = stamp; static_cast<int>(uValue(rtabmap_.getStatistics().data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f))!=0;
rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose);
poseMsg.pose.covariance; if(localized || !pubLocPoseOnlyWhenLocalizing_)
const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance(); {
memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double)); geometry_msgs::PoseWithCovarianceStamped poseMsg;
localizationPosePub_.publish(poseMsg); poseMsg.header.frame_id = mapFrameId_;
poseMsg.header.stamp = stamp;
rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose);
poseMsg.pose.covariance;
if(!rtabmap_.getStatistics().localizationCovariance().empty())
{
const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance();
memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double));
}
else
{
// Not yet localized, publish large covariance
poseMsg.pose.covariance.data()[0] = 9999;
poseMsg.pose.covariance.data()[7] = 9999;
poseMsg.pose.covariance.data()[14] = twoDMapping_?rtabmap::Registration::COVARIANCE_LINEAR_EPSILON:9999;
poseMsg.pose.covariance.data()[21] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999;
poseMsg.pose.covariance.data()[28] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999;
poseMsg.pose.covariance.data()[35] = 9999;
}
localizationPosePub_.publish(poseMsg);
}
} }
std::map<int, rtabmap::Transform> filteredPoses(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end()); std::map<int, rtabmap::Transform> filteredPoses(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());