add logs and minor fixes (#146)

* add logs and minor fixes

Signed-off-by: Alberto Soragna <[email protected]>

* use >0 rather than ==1 in comparison

Signed-off-by: Alberto Soragna <[email protected]>

---------

Signed-off-by: Alberto Soragna <[email protected]>

Author: alsora

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -408,23 +408,35 @@ class IntraProcessManager
     if (service_it == services_.end()) {
       auto cli = get_client_intra_process(intra_process_client_id);
       auto warning_msg =
-        "Intra-process service gone out of scope. "
+        "Intra-process service not found. "
         "Do inter-process requests.\n"
         "Client service name: " + std::string(cli->get_service_name());
       RCLCPP_WARN(rclcpp::get_logger("rclcpp"), warning_msg.c_str());
       return;
     }
     auto service_intra_process_base = service_it->second.lock();
-    if (service_intra_process_base) {
-      auto service = std::dynamic_pointer_cast<
-        rclcpp::experimental::ServiceIntraProcess<ServiceT>>(service_intra_process_base);
-      if (service) {
-        service->store_intra_process_request(
-          intra_process_client_id, std::move(request));
-      }
-    } else {
+    if (!service_intra_process_base) {
       services_.erase(service_it);
+      auto cli = get_client_intra_process(intra_process_client_id);
+      auto warning_msg =
+        "Intra-process service gone out of scope. "
+        "Do inter-process requests.\n"
+        "Client service name: " + std::string(cli->get_service_name());
+      RCLCPP_WARN(rclcpp::get_logger("rclcpp"), warning_msg.c_str());
+      return;
+    }
+    auto service = std::dynamic_pointer_cast<
+      rclcpp::experimental::ServiceIntraProcess<ServiceT>>(service_intra_process_base);
+    if (!service) {
+      auto cli = get_client_intra_process(intra_process_client_id);
+      auto warning_msg =
+        "Intra-process failed to cast server. "
+        "Client service name: " + std::string(cli->get_service_name());
+      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), warning_msg.c_str());
+      return;
     }
+
+    service->store_intra_process_request(intra_process_client_id, std::move(request));
   }
 
   template<

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -398,20 +398,6 @@ IntraProcessManager::get_service_intra_process(uint64_t intra_process_service_id
   }
 }
 
-bool
-IntraProcessManager::service_is_available(uint64_t intra_process_client_id)
-{
-  std::shared_lock<std::shared_timed_mutex> lock(mutex_);
-
-  auto service_it = clients_to_services_.find(intra_process_client_id);
-
-  if (service_it != clients_to_services_.end()) {
-    // A server matching the client has been found
-    return true;
-  }
-  return false;
-}
-
 ClientIntraProcessBase::SharedPtr
 IntraProcessManager::get_client_intra_process(uint64_t intra_process_client_id)
 {
@@ -478,16 +464,18 @@ IntraProcessManager::get_action_server_intra_process(
 }
 
 bool
-IntraProcessManager::action_server_is_available(uint64_t ipc_action_client_id)
+IntraProcessManager::service_is_available(uint64_t intra_process_client_id)
 {
   std::shared_lock<std::shared_timed_mutex> lock(mutex_);
-  auto action_service_it = action_clients_to_servers_.find(ipc_action_client_id);
 
-  if (action_service_it != action_clients_to_servers_.end()) {
-    // An action server matching the action client has been found
-    return true;
-  }
-  return false;
+  return clients_to_services_.count(intra_process_client_id) > 0u;
+}
+
+bool
+IntraProcessManager::action_server_is_available(uint64_t ipc_action_client_id)
+{
+  std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+  return action_clients_to_servers_.count(ipc_action_client_id) > 0u;
 }
 
 uint64_t

rclcpp/src/rclcpp/parameter_service.cpp

@@ -41,15 +41,21 @@ ParameterService::ParameterService(
       const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
     {
+      if (request->names.empty()) {
+        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Received empty get_parameters request");
+        return;
+      }
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Received get_parameters request (%s...)", request->names[0].c_str());
+
       try {
         auto parameters = node_params->get_parameters(request->names);
         for (const auto & param : parameters) {
           response->values.push_back(param.get_value_message());
         }
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
+        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to get parameters (%s...) : %s", request->names[0].c_str(), ex.what());
       } catch (const rclcpp::exceptions::ParameterUninitializedException & ex) {
-        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
+        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to get parameter (%s...): %s", request->names[0].c_str(), ex.what());
       }
     },
     qos_profile, nullptr);
@@ -70,7 +76,7 @@ ParameterService::ParameterService(
             return static_cast<rclcpp::ParameterType>(type);
           });
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
+        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
       }
     },
     qos_profile, nullptr);
@@ -83,6 +89,12 @@ ParameterService::ParameterService(
       const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
     {
+      if (request->parameters.empty()) {
+        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Received empty set_parameters request");
+        return;
+      }
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Received set_parameters request (%s...)", request->parameters[0].name.c_str());
+
       // Set parameters one-by-one, since there's no way to return a partial result if
       // set_parameters() fails.
       auto result = rcl_interfaces::msg::SetParametersResult();
@@ -91,7 +103,7 @@ ParameterService::ParameterService(
           result = node_params->set_parameters_atomically(
             {rclcpp::Parameter::from_parameter_msg(p)});
         } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-          RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what());
+          RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to set parameter (%s...): %s", request->parameters[0].name.c_str(), ex.what());
           result.successful = false;
           result.reason = ex.what();
         }
@@ -119,7 +131,7 @@ ParameterService::ParameterService(
         auto result = node_params->set_parameters_atomically(pvariants);
         response->result = result;
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(
+        RCLCPP_ERROR(
           rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what());
         response->result.successful = false;
         response->result.reason = "One or more parameters were not declared before setting";
@@ -139,7 +151,7 @@ ParameterService::ParameterService(
         auto descriptors = node_params->describe_parameters(request->names);
         response->descriptors = descriptors;
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
+        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
       }
     },
     qos_profile, nullptr);