add logs and minor fixes (#146)

* add logs and minor fixes Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com> * use >0 rather than ==1 in comparison Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com> --------- Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>
irobot-ros · Apr 17, 2024 · 2b92ba5 · 2b92ba5
1 parent 9177002
commit 2b92ba5
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 29 deletions.
diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp
@@ -402,23 +402,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<

diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp
@@ -468,26 +468,14 @@ 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;
+ 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_);
- 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 action_clients_to_servers_.count(ipc_action_client_id) > 0u;
 }
 
 uint64_t

diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/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);