ROS2 Dashing - How to set parameters of a lifecycle_node via another node

asked 2019-10-02 15:19:13 -0600

ignacio gravatar image

The problem: Change parameters from a lifecycle_node (let's call him original_node) without node shut down.

My approach: Reconfigure parameters during on_configure state by creating another node (let's say reconfigure_node), that is a client from original_node/set_parameters service and then call async_send_request(). So far I am trying to send new parameters values to original_node

Problems:

  • This approach only "works" when I use async_send_request() inside a wall_timer and call rclcpp::spin() in main;
  • When I do Ctrl + C, I receive message [INFO] [rclcpp]: signal_handler(signal_value=2) but process keeps running;
  • If I use spin_once() or spin_some() and call async_send_request() inside constructor, nothing happens.
  • When running on debug mode it stops saying [DEBUG] [rcl]: Client sending service request

To reproduce problem run original_node and and then ros2 run reconfigure_parameters reconfigure_parameters_node __node:=original_node __params:=simple_yaml.yaml

Expected result:

  • original_node publishing asd without the wall_timer in reconfigure_node class.

Questions:

  • What am I missing?
  • Is it possible to do the same using SyncParametersClient instead?

Code:

original_node

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses a fancy C++11 lambda
 * function to shorten the callback syntax, at the expense of making the
 * code somewhat more difficult to understand at first glance. */

class OriginalNode : public rclcpp::Node
{
public:
  OriginalNode()
  : Node("original_node"), count_(0)
  {
    this->declare_parameter("type","empty");
    auto timer_callback =
      [this]() -> void {
        auto message = std_msgs::msg::String();
        rclcpp::Parameter param_type = this->get_parameter("type");
        message.data = param_type.as_string() + "- " + std::to_string(this->count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
      };
    timer_ = this->create_wall_timer(500ms, timer_callback);
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<OriginalNode>());
  rclcpp::shutdown();
  return 0;
}

reconfigure_parameters.hpp:

// Reconfigure Header
#include <chrono>
#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using Response = rcl_interfaces::srv::SetParameters_Response::SharedPtr;

class ReconfigureParameters : public rclcpp::Node
{
public:
    ReconfigureParameters(std::string name = "reconfigure_parameters_node", const rclcpp::NodeOptions & options = (
        rclcpp::NodeOptions()
        .allow_undeclared_parameters(true)
        .automatically_declare_parameters_from_overrides(true)
        .start_parameter_event_publisher(true)
    ));
    Response sendParameters();

private:
  std::vector<std::string> params_names_vector;
  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client;
  rclcpp::SyncParametersClient::SharedPtr client_sync;
  rcl_interfaces::srv::SetParameters_Request::SharedPtr request;
  rclcpp::TimerBase::SharedPtr timer;
  Response response;
};

reconfigure_parameters.cpp

#include "reconfigure_parameters.hpp"

using Response = rcl_interfaces::srv::SetParameters_Response::SharedPtr;

ReconfigureParameters::ReconfigureParameters(std::string name, const rclcpp::NodeOptions & options) :
    Node(name, options)
{
    client = this->create_client<rcl_interfaces::srv::SetParameters>(std::string(this->get_name()) + "/set_parameters");
    request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();

    while(!client->wait_for_service(std::chrono::seconds(1)))
    {
      if(!rclcpp::ok())
        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");

      RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
    }

    auto param_names = this->list_parameters({}, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE);

    for(size_t i = 0; i < param_names.names.size(); ++i)
      params_names_vector.push_back(param_names.names[i]);

    auto timer_callback = [this]() -> void {response = sendParameters();};
    timer = this->create_wall_timer(std::chrono::milliseconds(200), timer_callback);
}

Response ReconfigureParameters::sendParameters()
{
    auto param = rcl_interfaces::msg::Parameter();
    for(auto& v : params_names_vector)
    {
      std::cout << this->get_parameter(v).get_name() << " - " << this->get_parameter(v).value_to_string() << std::endl;
      param.name = this->get_parameter ...
(more)
edit retag flag offensive close merge delete