ROS2 Parameter Change and publishing to /parameter_events

asked 2021-06-15 23:27:08 -0500

GusBus gravatar image

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

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

In my callback now, If I return the response as true, the parameter updates and there is a new message published on the /parameter_events topic If I return the response as false, the parameter does not update and no new message is published on the /parameter_events 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:

  • Where are the parameters actually being stored and where in the code are they being updated on a successful call of a set_parameter service?
  • Where is the publish to /parameter_events happening and does that have anything to do with the actual value stored in the parameter, or is it purely for notifications on parameter changes? If I publish a message directly to the /parameter_events topic using a different node and mimic the message I see published on a successful service call. No parameters are changed, which I half expected (probably stupidly)

Insight into my current set up:

param_callback - Main Node, that registers parameters we are changing and printing to screen param_client - Node that sets up a client and calls the /param_callback/set_parameters service param_publisher - Node that sets up a publisher and publishes directly to the /parameter_events 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 ...
(more)
edit retag flag offensive close merge delete