diff --git a/README.md b/README.md index e6f7de588..7a5e67093 100644 --- a/README.md +++ b/README.md @@ -282,6 +282,10 @@ The following settings and options are exposed to you. My default configuration `minimum_travel_distance` - Minimum distance of travel before processing a new scan +`minimum_travel_heading` - Minimum changing in heading to justify an update. + +`check_min_dist_and_heading_precisely` - Whether to always check if either *`minimum_travel_distance`* or *`minimum_travel_heading`* is satisfied. With the default value *`false`*, the behavior suits most cases where, for example, rotational odometry is poor. + `localization_on_configure` - Set to true to set the localization mode to localization during node on_configure transition. Set to false to set the localization mode to mapping instead. Only applies to `map_and_localization_slam_toolbox` node. ## Matcher Params @@ -290,8 +294,6 @@ The following settings and options are exposed to you. My default configuration `use_scan_barycenter` - Whether to use the barycenter or scan pose -`minimum_travel_heading` - Minimum changing in heading to justify an update - `scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode `scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer diff --git a/config/mapper_params_lifelong.yaml b/config/mapper_params_lifelong.yaml index 833b3058b..c1ff7075d 100644 --- a/config/mapper_params_lifelong.yaml +++ b/config/mapper_params_lifelong.yaml @@ -53,6 +53,7 @@ slam_toolbox: use_scan_barycenter: true minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 + check_min_dist_and_heading_precisely: false scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 link_match_minimum_response_fine: 0.1 diff --git a/config/mapper_params_localization.yaml b/config/mapper_params_localization.yaml index 589435f6a..ea4047ab7 100644 --- a/config/mapper_params_localization.yaml +++ b/config/mapper_params_localization.yaml @@ -36,6 +36,7 @@ slam_toolbox: use_scan_barycenter: true minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 + check_min_dist_and_heading_precisely: false scan_buffer_size: 3 scan_buffer_maximum_scan_distance: 10.0 link_match_minimum_response_fine: 0.1 diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml index fbd918224..4d9dba2e0 100644 --- a/config/mapper_params_offline.yaml +++ b/config/mapper_params_offline.yaml @@ -35,6 +35,7 @@ slam_toolbox: use_scan_barycenter: true minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 + check_min_dist_and_heading_precisely: false scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 link_match_minimum_response_fine: 0.1 diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index 78789ad74..0895abd9e 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -43,6 +43,7 @@ slam_toolbox: use_scan_barycenter: true minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 + check_min_dist_and_heading_precisely: false scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 link_match_minimum_response_fine: 0.1 diff --git a/config/mapper_params_online_multi_async.yaml b/config/mapper_params_online_multi_async.yaml index b2cc16c24..9b8e22074 100644 --- a/config/mapper_params_online_multi_async.yaml +++ b/config/mapper_params_online_multi_async.yaml @@ -40,6 +40,7 @@ $(var namespace)/slam_toolbox: use_scan_barycenter: true minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 + check_min_dist_and_heading_precisely: false scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 link_match_minimum_response_fine: 0.1 @@ -70,4 +71,4 @@ $(var namespace)/slam_toolbox: coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 - use_response_expansion: true \ No newline at end of file + use_response_expansion: true diff --git a/config/mapper_params_online_sync.yaml b/config/mapper_params_online_sync.yaml index 4c0f31c10..73a4ac7b1 100644 --- a/config/mapper_params_online_sync.yaml +++ b/config/mapper_params_online_sync.yaml @@ -43,6 +43,7 @@ slam_toolbox: use_scan_barycenter: true minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 + check_min_dist_and_heading_precisely: false scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 link_match_minimum_response_fine: 0.1 diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index f6b6991d6..6edc19477 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -174,6 +174,7 @@ class SlamToolbox : public rclcpp_lifecycle::LifecycleNode double yaw_covariance_scale_; bool first_measurement_, enable_interactive_mode_; bool restamp_tf_; + bool check_min_dist_and_heading_precisely_; // Book keeping std::unique_ptr smapper_; diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 152791265..92e02c014 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2424,6 +2424,7 @@ class KARTO_EXPORT Mapper : public Module double getParamMinimumTimeInterval(); double getParamMinimumTravelDistance(); double getParamMinimumTravelHeading(); + double getParamMinimumTravelHeadingInRadians(); int getParamScanBufferSize(); double getParamScanBufferMaximumScanDistance(); double getParamLinkMatchMinimumResponseFine(); diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index e96940421..6ef0688b4 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -2333,6 +2333,11 @@ double Mapper::getParamMinimumTravelHeading() return math::RadiansToDegrees(static_cast(m_pMinimumTravelHeading->GetValue())); } +double Mapper::getParamMinimumTravelHeadingInRadians() +{ + return static_cast(m_pMinimumTravelHeading->GetValue()); +} + int Mapper::getParamScanBufferSize() { return static_cast(m_pScanBufferSize->GetValue()); diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index e36a2803e..f54891b63 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -411,6 +411,13 @@ void SlamToolbox::setParams() tmp_val = this->get_parameter("minimum_time_interval").as_double(); minimum_time_interval_ = rclcpp::Duration::from_seconds(tmp_val); + check_min_dist_and_heading_precisely_ = false; + if (!this->has_parameter("check_min_dist_and_heading_precisely")) { + this->declare_parameter("check_min_dist_and_heading_precisely", check_min_dist_and_heading_precisely_); + } + check_min_dist_and_heading_precisely_ = + this->get_parameter("check_min_dist_and_heading_precisely").as_bool(); + bool debug = false; if (!this->has_parameter("debug_logging")) { this->declare_parameter("debug_logging", debug); @@ -758,6 +765,8 @@ bool SlamToolbox::shouldProcessScan( static double min_dist2 = smapper_->getMapper()->getParamMinimumTravelDistance() * smapper_->getMapper()->getParamMinimumTravelDistance(); + static double min_rotation = + smapper_->getMapper()->getParamMinimumTravelHeadingInRadians(); static int scan_ctr = 0; scan_ctr++; @@ -784,9 +793,21 @@ bool SlamToolbox::shouldProcessScan( return false; } - // check moved enough, within 10% for correction error + // for initial stabilization + if (scan_ctr < 5) { + return false; + } + + // check if the movement is enough const double dist2 = last_pose.SquaredDistance(pose); - if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) { + if (check_min_dist_and_heading_precisely_) { + const double heading_diff = + fabs(math::NormalizeAngle(pose.GetHeading() - last_pose.GetHeading())); + if (dist2 < min_dist2 && heading_diff < min_rotation) { + return false; + } + } else if (dist2 < 0.8 * min_dist2) { + // within 10% for correction error, min heading is occasionally checked in the mapper return false; }