From 94b96956642de3bcb755e3b80537d2f97d7a20a9 Mon Sep 17 00:00:00 2001 From: MartiBolet <43337758+MartiBolet@users.noreply.github.com> Date: Thu, 9 Jun 2022 20:23:11 +0200 Subject: [PATCH] Get parameters on configure transition addressing #2985 (#3000) * Get parameters on configure transition Signed-off-by: MartiBolet * Remove past setting of parameters Signed-off-by: MartiBolet * Expose transition functions to public for test Signed-off-by: MartiBolet --- .../include/nav2_map_server/map_saver.hpp | 2 +- nav2_map_server/src/map_saver/main_cli.cpp | 1 + nav2_map_server/src/map_saver/map_saver.cpp | 17 +++++++++++------ 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 6863844c7b5..bfeb986c624 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -56,7 +56,6 @@ class MapSaver : public nav2_util::LifecycleNode const std::string & map_topic, const SaveParameters & save_parameters); -protected: /** * @brief Sets up map saving service * @param state Lifecycle Node's state @@ -88,6 +87,7 @@ class MapSaver : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; +protected: /** * @brief Map saving service callback * @param request_header Service request header diff --git a/nav2_map_server/src/map_saver/main_cli.cpp b/nav2_map_server/src/map_saver/main_cli.cpp index 09cf039f441..48d53450606 100644 --- a/nav2_map_server/src/map_saver/main_cli.cpp +++ b/nav2_map_server/src/map_saver/main_cli.cpp @@ -164,6 +164,7 @@ int main(int argc, char ** argv) int retcode; try { auto map_saver = std::make_shared(); + map_saver->on_configure(rclcpp_lifecycle::State()); if (map_saver->saveMapTopicToFile(map_topic, save_parameters)) { retcode = 0; } else { diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index d79005a567b..1517e787526 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -46,12 +46,11 @@ MapSaver::MapSaver(const rclcpp::NodeOptions & options) { RCLCPP_INFO(get_logger(), "Creating"); - save_map_timeout_ = std::make_shared( - rclcpp::Duration::from_seconds(declare_parameter("save_map_timeout", 2.0))); - - free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), - occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); - map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", true); + // Declare the node parameters + declare_parameter("save_map_timeout", 2.0); + declare_parameter("free_thresh_default", 0.25); + declare_parameter("occupied_thresh_default", 0.65); + declare_parameter("map_subscribe_transient_local", true); } MapSaver::~MapSaver() @@ -66,6 +65,12 @@ MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) // Make name prefix for services const std::string service_prefix = get_name() + std::string("/"); + save_map_timeout_ = std::make_shared( + rclcpp::Duration::from_seconds(get_parameter("save_map_timeout").as_double())); + free_thresh_default_ = get_parameter("free_thresh_default").as_double(); + occupied_thresh_default_ = get_parameter("occupied_thresh_default").as_double(); + map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool(); + // Create a service that saves the occupancy grid from map topic to a file save_map_service_ = create_service( service_prefix + save_map_service_name_,