Skip to content

Publish pose with covariance in localization mode. (ros2 branch) #511

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SteveMacenski merged 1 commit into from
Jul 11, 2022
Merged

Publish pose with covariance in localization mode. (ros2 branch) #511

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion lib/karto_sdk/include/karto_sdk/Mapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2007,7 +2007,7 @@ class KARTO_EXPORT Mapper : public Module
kt_bool ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId, Matrix3 * covariance = nullptr);
kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer = false, Matrix3 * covariance = nullptr);
kt_bool ProcessLocalization(LocalizedRangeScan * pScan);
kt_bool ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan> *);
void AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex<LocalizedRangeScan> * scan_vertex);
void ClearLocalizationBuffer();
Expand Down
13 changes: 8 additions & 5 deletions lib/karto_sdk/src/Mapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2828,7 +2828,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad
return false;
}

kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance)
{
if (pScan == NULL) {
return false;
Expand Down Expand Up @@ -2866,17 +2866,20 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
return false;
}

Matrix3 covariance;
covariance.SetToIdentity();
Matrix3 cov;
cov.SetToIdentity();

// correct scan (if not first scan)
if (m_pUseScanMatching->GetValue() && pLastScan != NULL) {
Pose2 bestPose;
m_pSequentialScanMatcher->MatchScan(pScan,
m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
bestPose,
covariance);
cov);
pScan->SetSensorPose(bestPose);
if (covariance) {
*covariance = cov;
}
}

// add scan to buffer and assign id
Expand All @@ -2886,7 +2889,7 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
if (m_pUseScanMatching->GetValue()) {
// add to graph
scan_vertex = m_pGraph->AddVertex(pScan);
m_pGraph->AddEdges(pScan, covariance);
m_pGraph->AddEdges(pScan, cov);

m_pMapperSensorManager->AddRunningScan(pScan);

Expand Down
10 changes: 8 additions & 2 deletions src/slam_toolbox_localization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,10 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan(
// Add the localized range scan to the smapper
boost::mutex::scoped_lock lock(smapper_mutex_);
bool processed = false, update_reprocessing_transform = false;

Matrix3 covariance;
covariance.SetToIdentity();

if (processor_type_ == PROCESS_NEAR_REGION) {
if (!process_near_pose_) {
RCLCPP_ERROR(get_logger(),
Expand All @@ -172,13 +176,13 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan(
range_scan->SetOdometricPose(*process_near_pose_);
range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
process_near_pose_.reset(nullptr);
processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true);
processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance);

// reset to localization mode
update_reprocessing_transform = true;
processor_type_ = PROCESS_LOCALIZATION;
} else if (processor_type_ == PROCESS_LOCALIZATION) {
processed = smapper_->getMapper()->ProcessLocalization(range_scan);
processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance);
update_reprocessing_transform = false;
} else {
RCLCPP_FATAL(get_logger(), "LocalizationSlamToolbox: "
Expand All @@ -194,6 +198,8 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan(
// compute our new transform
setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose,
scan->header.stamp, update_reprocessing_transform);

publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
}

return range_scan;
Expand Down