diff --git a/README.md b/README.md index e9c332056..573bc3336 100644 --- a/README.md +++ b/README.md @@ -177,8 +177,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 @@ -239,6 +241,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 diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 9274ba545..46c3c90fc 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -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); @@ -130,6 +134,7 @@ class SlamToolbox : public rclcpp::Node std::unique_ptr> scan_filter_; std::shared_ptr> sst_; std::shared_ptr> sstm_; + std::shared_ptr> pose_pub_; std::shared_ptr> ssMap_; std::shared_ptr> ssPauseMeasurements_; std::shared_ptr> ssSerialize_; @@ -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 diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index ae6c8cad3..8c3cab01c 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1996,7 +1996,7 @@ 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 @@ -2004,10 +2004,10 @@ class KARTO_EXPORT Mapper : public Module 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 ProcessLocalization(LocalizedRangeScan * pScan); + 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, Matrix3 * covariance = nullptr); kt_bool RemoveNodeFromGraph(Vertex *); void AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex * scan_vertex); void ClearLocalizationBuffer(); diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 27c029303..e8cb71058 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -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(); @@ -2705,8 +2705,8 @@ 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) { @@ -2714,8 +2714,11 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan) 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 @@ -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); @@ -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(); @@ -2773,8 +2776,8 @@ 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) { @@ -2782,12 +2785,16 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad 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); @@ -2795,7 +2802,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad 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); @@ -2821,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; @@ -2859,8 +2866,8 @@ 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) { @@ -2868,8 +2875,11 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan) 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 @@ -2879,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); @@ -3012,7 +3022,8 @@ kt_bool Mapper::RemoveNodeFromGraph(Vertex * 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(); @@ -3038,8 +3049,8 @@ 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) { @@ -3047,11 +3058,14 @@ kt_bool Mapper::ProcessAgainstNode( 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); @@ -3059,7 +3073,7 @@ kt_bool Mapper::ProcessAgainstNode( 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); @@ -3081,10 +3095,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); } /** diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index c9ab7a529..f3d3ac5f5 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -144,6 +144,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_); @@ -181,6 +187,8 @@ void SlamToolbox::setROSInterfaces() tfL_ = std::make_unique(*tf_); tfB_ = std::make_unique(shared_from_this()); + pose_pub_ = this->create_publisher( + "pose", 10); sst_ = this->create_publisher( map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); sstm_ = this->create_publisher( @@ -538,10 +546,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) { @@ -554,7 +565,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 { @@ -573,6 +585,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; @@ -581,6 +595,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 request_header, diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index e8dfbf6b9..50469b6eb 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -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(), @@ -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: " @@ -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;