testSetupNode.cpp

Go to the documentation of this file.

#include <testSetupNode/testSetupNode.hpp>
#include <rclcpp/rclcpp.hpp>
#include <utility>
#include <cmath>
#include <cstdint>
#include <memory>
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include <galaxis/Errors.hpp>
#include <galaxis/linatypes.hpp>
 
using namespace galaxis::servoSettings;
using namespace galaxis::motorSettings;
// phth
using namespace std::chrono_literals;
 
testSetupNode::testSetupNode(void) :
    rclcpp::Node("testSetupNode") {
}
 
bool testSetupNode::send_settings_(std::shared_ptr<std::map<std::string, std::vector<rclcpp::Parameter>>> p_settingsGUIParameterMap) {
  m_sendMtx.lock();
 
  bool status = true; // successful
 
  for (auto const& [nodeName, parametersVec] : *p_settingsGUIParameterMap) {
    // 1. define a ParametersClient (which is a wrapper around parameter services) 
    // which can get and set the parameters of each ROS node
    auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(this, "/" + nodeName);
 
    if(!parameters_client->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service of node %s. Exiting.", nodeName.c_str());
        rclcpp::shutdown();
      }
 
      RCLCPP_INFO(this->get_logger(), "Service for Node %s not available, abort sending parameters to this node...", nodeName.c_str());
      status = false;
      continue;
    }
      
    if(parameters_client == nullptr) {
      RCLCPP_ERROR(this->get_logger(), "parameters_client is null");
      // throw galaxis::Errors::POINTER_ERROR();
      status = false;
      continue;
    }
 
    // 2. Set parameters, if they were changed.
    for (auto & parameter : parametersVec)
    {
      rclcpp::ParameterValue previousParameterValue = parameters_client->get_parameters({parameter.get_name()}).at(0).get_parameter_value();
      if (parameter.get_parameter_value() != previousParameterValue) {
        auto set_parameters_results = parameters_client->set_parameters(
          {
            rclcpp::Parameter(parameter.get_name(), parameter.get_parameter_value()),
          }
        );
      
        // Check to see if the parameter was set. Log updated parameters.
        for (auto & result : set_parameters_results) {
          if (!result.successful) {
            RCLCPP_ERROR(this->get_logger(), "Failed to set parameter: %s", result.reason.c_str());
          }
          else {
            RCLCPP_WARN(this->get_logger(), "[New Parameter] Set & changed parameter successfully: %s = %s", parameter.get_name().c_str(), parameter.value_to_string().c_str());
          }
        }
      }
      else {
        RCLCPP_INFO(this->get_logger(), "No change: %s = %s", parameter.get_name().c_str(), parameter.value_to_string().c_str());
      }
    }
    RCLCPP_INFO(this->get_logger(), "====================================== END OF SET PARAMETER LOG %s ======================================", nodeName.c_str());
  }
  m_sendMtx.unlock();
 
  return status;
}