Check for ACTIVE state in services

ros-navigation · Dec 6, 2019 · 9824627 · 9824627
1 parent 9d04d07
commit 9824627
Showing 1 changed file with 13 additions and 5 deletions.
diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp
@@ -131,7 +131,7 @@ bool OccGridLoader::loadMapFromYaml(
   std::string yaml_file,
   std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response)
 {
-  if (yaml_file == "") {
+  if (yaml_file.empty()) {
     RCLCPP_ERROR(node_->get_logger(), "YAML file name is empty, can't load!");
     response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_ID_DOES_NOT_EXIST;
     return false;
@@ -182,6 +182,11 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St
     const std::shared_ptr<rmw_request_id_t>/*request_header*/,
     const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/,
     std::shared_ptr<nav_msgs::srv::GetMap::Response> response) -> void {
+      if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+        RCLCPP_WARN(node_->get_logger(),
+          "Received GetMap request but not in ACTIVE state, ignoring!");
+        return;
+      }
       RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling GetMap request");
       response->map = *msg_;
     };
@@ -194,6 +199,12 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St
     const std::shared_ptr<rmw_request_id_t>/*request_header*/,
     const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,
     std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response) -> void {
+      // if not in ACTIVE state, ignore request
+      if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+        RCLCPP_WARN(node_->get_logger(),
+          "Received LoadMap request but not in ACTIVE state, ignoring!");
+        return;
+      }
       RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling LoadMap request");
       // Check if map_id is a file
       if (request->type != nav2_msgs::srv::LoadMap::Request::TYPE_FILE) {
@@ -206,10 +217,7 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St
       if (loadMapFromYaml(request->map_id, response)) {
         response->map = *msg_;
         response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS;
-        // if in ACTIVE state, publish map
-        if (node_->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
-          occ_pub_->publish(*msg_);
-        }
+        occ_pub_->publish(*msg_);  // publish new map
       }
     };