ROS2 Dashing - How to set parameters of a lifecycle_node via another node
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 awall_timer
and callrclcpp::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()
orspin_some()
and callasync_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 thewall_timer
inreconfigure_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 ...