Skip to content

Commit

Permalink
Publish PoseWithCovarianceStamped message with each scan match. (#504)
Browse files Browse the repository at this point in the history 
* Publish PoseWithCovarianceStamped message with each scan match.

* Refactor code to return covariance as a output parameter instead of storing in the range scan object.

* Linting fixes.
  • Loading branch information
@malban @SteveMacenski
malban authored and SteveMacenski committed Jun 22, 2022
1 parent acb6ea2 commit ae68b85
Show file tree
Hide file tree
Showing 5 changed files with 88 additions and 25 deletions.
8 changes: 7 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,8 +174,10 @@ The following are the services/topics that are exposed for use. See the rviz plu

## Published topics

| map | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency |
| Topic | Type | Description |
|-----|----|----|
| map | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency |
| pose | `geometry_msgs/PoseWithCovarianceStamped` | pose of the base_frame in the configured map_frame along with the covariance calculated from the scan match |

## Exposed Services

Expand Down Expand Up @@ -236,6 +238,10 @@ The following settings and options are exposed to you. My default configuration

`enable_interactive_mode` - Whether or not to allow for interactive mode to be enabled. Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode. As a result the memory for the process will increase. This is manually disabled in localization and lifelong modes since they would increase the memory utilization over time. Valid for either mapping or continued mapping modes.

`position_covariance_scale` - Amount to scale position covariance when publishing pose from scan match. This can be used to tune the influence of the pose position in a downstream localization filter. The covariance represents the uncertainty of the measurement, so scaling up the covariance will result in the pose position having less influence on downstream filters. Default: 1.0

`yaw_covariance_scale` - Amount to scale yaw covariance when publishing pose from scan match. See description of position_covariance_scale. Default: 1.0

`resolution` - Resolution of the 2D occupancy map to generate

`max_laser_range` - Maximum laser range to use for 2D occupancy map rastering
Expand Down
7 changes: 7 additions & 0 deletions include/slam_toolbox/slam_toolbox_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,10 @@ class SlamToolbox : public rclcpp::Node
bool shouldProcessScan(
const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
const karto::Pose2 & pose);
void publishPose(
const Pose2 & pose,
const Matrix3 & cov,
const rclcpp::Time & t);

// pausing bits
bool isPaused(const PausedApplication & app);
Expand All @@ -130,6 +134,7 @@ class SlamToolbox : public rclcpp::Node
std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> scan_filter_;
std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> sst_;
std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> sstm_;
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>> pose_pub_;
std::shared_ptr<rclcpp::Service<nav_msgs::srv::GetMap>> ssMap_;
std::shared_ptr<rclcpp::Service<slam_toolbox::srv::Pause>> ssPauseMeasurements_;
std::shared_ptr<rclcpp::Service<slam_toolbox::srv::SerializePoseGraph>> ssSerialize_;
Expand All @@ -142,6 +147,8 @@ class SlamToolbox : public rclcpp::Node
int throttle_scans_;

double resolution_;
double position_covariance_scale_;
double yaw_covariance_scale_;
bool first_measurement_, enable_interactive_mode_;

// Book keeping
Expand Down
8 changes: 4 additions & 4 deletions lib/karto_sdk/include/karto_sdk/Mapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1996,17 +1996,17 @@ class KARTO_EXPORT Mapper : public Module
*
* @return true if the scan was added successfully, false otherwise
*/
virtual kt_bool Process(LocalizedRangeScan * pScan);
virtual kt_bool Process(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);

/**
* Process an Object
*/
virtual kt_bool Process(Object * pObject);

// processors
kt_bool ProcessAtDock(LocalizedRangeScan * pScan);
kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId);
kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer = false);
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 RemoveNodeFromGraph(Vertex<LocalizedRangeScan> *);
void AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex<LocalizedRangeScan> * scan_vertex);
Expand Down
45 changes: 28 additions & 17 deletions lib/karto_sdk/src/Mapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2676,7 +2676,7 @@ kt_bool Mapper::Process(Object * /*pObject*/) // NOLINT
return true;
}

kt_bool Mapper::Process(LocalizedRangeScan * pScan)
kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance)
{
if (pScan != NULL) {
karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder();
Expand Down Expand Up @@ -2705,17 +2705,20 @@ kt_bool Mapper::Process(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 @@ -2724,7 +2727,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan)
if (m_pUseScanMatching->GetValue()) {
// add to graph
m_pGraph->AddVertex(pScan);
m_pGraph->AddEdges(pScan, covariance);
m_pGraph->AddEdges(pScan, cov);

m_pMapperSensorManager->AddRunningScan(pScan);

Expand All @@ -2745,7 +2748,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan)
return false;
}

kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer)
kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer, Matrix3 * covariance)
{
if (pScan != NULL) {
karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder();
Expand Down Expand Up @@ -2773,29 +2776,33 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad
m_pMapperSensorManager->SetLastScan(pLastScan);
}

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);
}

pScan->SetOdometricPose(pScan->GetCorrectedPose());

if (covariance) {
*covariance = cov;
}

// add scan to buffer and assign id
m_pMapperSensorManager->AddScan(pScan);

Vertex<LocalizedRangeScan> * scan_vertex = NULL;
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 Expand Up @@ -3012,7 +3019,8 @@ kt_bool Mapper::RemoveNodeFromGraph(Vertex<LocalizedRangeScan> * vertex_to_remov

kt_bool Mapper::ProcessAgainstNode(
LocalizedRangeScan * pScan,
const int & nodeId)
const int & nodeId,
Matrix3 * covariance)
{
if (pScan != NULL) {
karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder();
Expand All @@ -3038,28 +3046,31 @@ kt_bool Mapper::ProcessAgainstNode(
m_pMapperSensorManager->AddRunningScan(pLastScan);
m_pMapperSensorManager->SetLastScan(pLastScan);

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);
}

pScan->SetOdometricPose(pScan->GetCorrectedPose());
if (covariance) {
*covariance = cov;
}

// add scan to buffer and assign id
m_pMapperSensorManager->AddScan(pScan);

if (m_pUseScanMatching->GetValue()) {
// add to graph
m_pGraph->AddVertex(pScan);
m_pGraph->AddEdges(pScan, covariance);
m_pGraph->AddEdges(pScan, cov);

m_pMapperSensorManager->AddRunningScan(pScan);

Expand All @@ -3081,10 +3092,10 @@ kt_bool Mapper::ProcessAgainstNode(
return false;
}

kt_bool Mapper::ProcessAtDock(LocalizedRangeScan * pScan)
kt_bool Mapper::ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance)
{
// Special case of processing against node where node is the starting point
return ProcessAgainstNode(pScan, 0);
return ProcessAgainstNode(pScan, 0, covariance);
}

/**
Expand Down
45 changes: 42 additions & 3 deletions src/slam_toolbox_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,12 @@ void SlamToolbox::setParams()
throttle_scans_ = 1;
throttle_scans_ = this->declare_parameter("throttle_scans", throttle_scans_);

position_covariance_scale_ = 1.0;
position_covariance_scale_ = this->declare_parameter("position_covariance_scale", position_covariance_scale_);

yaw_covariance_scale_ = 1.0;
yaw_covariance_scale_ = this->declare_parameter("yaw_covariance_scale", yaw_covariance_scale_);

enable_interactive_mode_ = false;
enable_interactive_mode_ = this->declare_parameter("enable_interactive_mode",
enable_interactive_mode_);
Expand Down Expand Up @@ -182,6 +188,8 @@ void SlamToolbox::setROSInterfaces()
tfL_ = std::make_unique<tf2_ros::TransformListener>(*tf_);
tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>(shared_from_this());

pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"pose", 10);
sst_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
sstm_ = this->create_publisher<nav_msgs::msg::MapMetaData>(
Expand Down Expand Up @@ -544,10 +552,13 @@ LocalizedRangeScan * SlamToolbox::addScan(
boost::mutex::scoped_lock lock(smapper_mutex_);
bool processed = false, update_reprocessing_transform = false;

Matrix3 covariance;
covariance.SetToIdentity();

if (processor_type_ == PROCESS) {
processed = smapper_->getMapper()->Process(range_scan);
processed = smapper_->getMapper()->Process(range_scan, &covariance);
} else if (processor_type_ == PROCESS_FIRST_NODE) {
processed = smapper_->getMapper()->ProcessAtDock(range_scan);
processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance);
processor_type_ = PROCESS;
update_reprocessing_transform = true;
} else if (processor_type_ == PROCESS_NEAR_REGION) {
Expand All @@ -560,7 +571,8 @@ LocalizedRangeScan * SlamToolbox::addScan(
range_scan->SetOdometricPose(*process_near_pose_);
range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
process_near_pose_.reset(nullptr);
processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan);
processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(
range_scan, false, &covariance);
update_reprocessing_transform = true;
processor_type_ = PROCESS;
} else {
Expand All @@ -579,6 +591,8 @@ LocalizedRangeScan * SlamToolbox::addScan(
setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose,
scan->header.stamp, update_reprocessing_transform);
dataset_->Add(range_scan);

publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
} else {
delete range_scan;
range_scan = nullptr;
Expand All @@ -587,6 +601,31 @@ LocalizedRangeScan * SlamToolbox::addScan(
return range_scan;
}

/*****************************************************************************/
void SlamToolbox::publishPose(
const Pose2 & pose,
const Matrix3 & cov,
const rclcpp::Time & t)
/*****************************************************************************/
{
geometry_msgs::msg::PoseWithCovarianceStamped pose_msg;
pose_msg.header.stamp = t;
pose_msg.header.frame_id = map_frame_;

tf2::Quaternion q(0., 0., 0., 1.0);
q.setRPY(0., 0., pose.GetHeading());
tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0));
tf2::toMsg(transform, pose_msg.pose.pose);

pose_msg.pose.covariance[0] = cov(0, 0) * position_covariance_scale_; // x
pose_msg.pose.covariance[1] = cov(0, 1) * position_covariance_scale_; // xy
pose_msg.pose.covariance[6] = cov(1, 0) * position_covariance_scale_; // xy
pose_msg.pose.covariance[7] = cov(1, 1) * position_covariance_scale_; // y
pose_msg.pose.covariance[35] = cov(2, 2) * yaw_covariance_scale_; // yaw

pose_pub_->publish(pose_msg);
}

/*****************************************************************************/
bool SlamToolbox::mapCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
Expand Down

0 comments on commit ae68b85

Please sign in to comment.