Robotics StackExchange | Archived questions

ROS2 Parameter Change and publishing to /parameter_events

I have been experimenting with ROS2 Params and have a question about the bidirectional nature of the /parameterevents topic and adding a parameterCallback using the addonsetparameters_callback().

So I can see that everytime the setparameters service is called we successfully go into the parameterCallback as long as we register it with the addonsetparameters_callback().

In my callback now, If I return the response as true, the parameter updates and there is a new message published on the /parameterevents topic If I return the response as false, the parameter does not update and no new message is published on the /parameterevents topic.

This is all desired behavior, but I am trying to understand what is happening under the hood and after sifting through a lot of the code I am coming up short handed.

So my questions are:

Insight into my current set up:

paramcallback - Main Node, that registers parameters we are changing and printing to screen paramclient - Node that sets up a client and calls the /paramcallback/setparameters service parampublisher - Node that sets up a publisher and publishes directly to the /parameterevents topic

param_callback

#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"

class TestParamsCallback : public rclcpp::Node
{
public:
    rcl_interfaces::msg::SetParametersResult parametersCallback(
    const std::vector<rclcpp::Parameter> &parameters)
{
    RCLCPP_INFO(this->get_logger(), "Entered Param Callback");
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = false;
    result.reason = "";
    for (const auto &param : parameters)
    {
        if (param.get_name() == "motor_device_port")
        {
            if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
            {
                if (param.as_string().rfind("/dev/tty", 0) == 0) // starts with
                {
                    motor_device_port_ = param.as_string();
                    result.successful = true;
                }
                else
                {
                    result.reason = "Not a Port";
                }
            }
            else
            {
                result.reason = "Incorrect Type";
            }
        }
        if (param.get_name() == "control_loop_frequency")
        {
            if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
            {
                if (param.as_int() >= 1 && param.as_int() < 1000)
                {
                    control_loop_frequency_ = param.as_int();
                    result.successful = true;
                }
                else
                {
                    result.reason = "Out of Range";
                }
            }
            else
            {
                result.reason = "Incorrect Type";
            }
        }
        if (param.get_name() == "simulation_mode")
        {
            if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
            {
                simulation_mode_ = param.as_bool();
                result.successful = true;
            }
            else
            {
                result.reason = "Incorrect Type";
            }
        }
    }
    return result;
}

    TestParamsCallback() : Node("param_callback")
    {
        this->declare_parameter("motor_device_port", "/dev/ttyUSB0");
        this->declare_parameter("control_loop_frequency", 100);
        this->declare_parameter("simulation_mode", false);

        motor_device_port_ = this->get_parameter("motor_device_port").as_string();
        control_loop_frequency_ = this->get_parameter("control_loop_frequency").as_int();
        simulation_mode_ = this->get_parameter("simulation_mode").as_bool();

        callback_handle_ = this->add_on_set_parameters_callback(
            std::bind(&TestParamsCallback::parametersCallback, this, std::placeholders::_1));

        _timer = this->create_wall_timer(std::chrono::seconds(1),
        std::bind(&TestParamsCallback::timer_callback, this)
        );
    }

    void timer_callback()
    {
        RCLCPP_INFO(this->get_logger(), "Motor Device Port: %s", this->get_parameter("motor_device_port").as_string().c_str());
        RCLCPP_INFO(this->get_logger(), "Control Loop Frequency: %d", this->get_parameter("control_loop_frequency").as_int());
        RCLCPP_INFO(this->get_logger(), "Simulation Mode: %s", this->get_parameter("simulation_mode").as_bool() ? "true":"false");
    }

private:
    std::string motor_device_port_;
    int control_loop_frequency_;
    bool simulation_mode_;

    OnSetParametersCallbackHandle::SharedPtr callback_handle_;

    rclcpp::TimerBase::SharedPtr _timer;
};


int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TestParamsCallback>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

param_client

#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"


class ParamClientNode : public rclcpp::Node
{
public:
    ParamClientNode() : Node("param_client")
    {
        this->declare_parameter("loop_hz", 75);

        auto param = rcl_interfaces::msg::Parameter();
        param.name = "control_loop_frequency";

        param.value.type = 2;
        param.value.integer_value = this->get_parameter("loop_hz").as_int();

        _threads.push_back(std::thread(std::bind(&ParamClientNode::callSetParameter, this, param)));
    }

    void callSetParameter(rcl_interfaces::msg::Parameter param)
    {
        auto client = this->create_client<rcl_interfaces::srv::SetParameters>("/param_callback2/set_parameters");
        while(!client->wait_for_service(std::chrono::seconds(1)))
        {
            RCLCPP_WARN(this->get_logger(), "Waiting for Server");
        }

        auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
        request->parameters.push_back(param);

        auto future = client->async_send_request(request);

        try
        {
            auto response = future.get();

            RCLCPP_INFO(this->get_logger(), "Call Response: %d with Reason: %s", response->results.begin()->successful, response->results.begin()->reason.c_str());
        }
        catch(const std::exception& e)
        {
            RCLCPP_ERROR(this->get_logger(), "Service Call Failed");
        }
    }

private:
    std::thread _thread1;
    std::vector<std::thread> _threads;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ParamClientNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

param_publisher

#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rcl_interfaces/msg/parameter.hpp"

class ParamPublisherNode : public rclcpp::Node 
{
public:
    ParamPublisherNode() : Node("param_publisher") 
    {   
        _param_publisher = this->create_publisher<rcl_interfaces::msg::ParameterEvent>("parameter_events", 10);
        _publisher_timer = this->create_wall_timer(std::chrono::seconds(5), std::bind(&ParamPublisherNode::timerCallback, this));
    }

    void timerCallback()
    {
        auto msg = rcl_interfaces::msg::ParameterEvent();
        auto param = rcl_interfaces::msg::Parameter();

        msg.stamp = this->get_clock()->now();
        msg.node = "/param_callback2";

        param.name = "control_loop_frequency";
        param.value.type = 2;
        param.value.integer_value = 66;

        msg.changed_parameters.push_back(param);

        _param_publisher->publish(msg);

        RCLCPP_INFO(this->get_logger(), "Published");

    }

private:

    rclcpp::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr _param_publisher;
    rclcpp::TimerBase::SharedPtr _publisher_timer;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<ParamPublisherNode>(); // MODIFY NAME

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Asked by GusBus on 2021-06-15 23:27:08 UTC

Comments

Answers