Fix param parsing

mrpt-ros-pkg · Aug 26, 2023 · 6fbcd85 · 6fbcd85
1 parent 9babc66
commit 6fbcd85
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 5 deletions.
diff --git a/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h b/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h
@@ -36,7 +36,7 @@
 class PFLocalizationCore : public mrpt::system::COutputLogger
 {
    public:
-	PFLocalizationCore() = default;
+	PFLocalizationCore();
 	virtual ~PFLocalizationCore() = default;
 
 	/** Parameters the filter will use to initialize or to run.

diff --git a/mrpt_pf_localization/include/mrpt_pf_localization_node.h b/mrpt_pf_localization/include/mrpt_pf_localization_node.h
@@ -88,6 +88,8 @@ class PFLocalizationNode : public rclcpp::Node
    private:
 	PFLocalizationCore core_;
 
+	rclcpp::TimerBase::SharedPtr timer_;
+
 	///
 	void reload_params_from_ros();
 

diff --git a/mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp b/mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp
@@ -174,6 +174,11 @@ void PFLocalizationCore::Parameters::load_from(
 	// class.
 }
 
+PFLocalizationCore::PFLocalizationCore()
+	: mrpt::system::COutputLogger("mrpt_pf_localization")
+{
+}
+
 void PFLocalizationCore::on_observation(const mrpt::obs::CObservation::Ptr& obs)
 {
 	auto tle = mrpt::system::CTimeLoggerEntry(profiler_, "on_observation");

diff --git a/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp b/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp
@@ -99,7 +99,7 @@ PFLocalizationNode::PFLocalizationNode(const rclcpp::NodeOptions& options)
 
 	// Create timer:
 	// ------------------------------------------
-	this->create_wall_timer(
+	timer_ = this->create_wall_timer(
 		std::chrono::microseconds(mrpt::round(1.0e6 / nodeParams_.rate_hz)),
 		[this]() { this->loop(); });
 }
@@ -140,9 +140,17 @@ void PFLocalizationNode::reload_params_from_ros()
 			const std::string childKey = name.substr(pos + 1);
 			name = childKey;
 
-			// Create YAML node:
-			(*targetYamlNode)[parentKey] = mrpt::containers::yaml::Map();
-			targetYamlNode = &(*targetYamlNode)[parentKey].asMap();
+			// Use subnode:
+			if (auto it = targetYamlNode->find(parentKey);
+				it == targetYamlNode->end())
+			{  // create new:
+				(*targetYamlNode)[parentKey] = mrpt::containers::yaml::Map();
+				targetYamlNode = &(*targetYamlNode)[parentKey].asMap();
+			}
+			else
+			{  // reuse
+				targetYamlNode = &it->second.asMap();
+			}
 		}
 
 		// Get param value: