Setting parameters with AsyncParametersClient fails

[nate-jackson]

I have a simple ros2 node that is listening for a parameter to be set by another node, and when it gets set, it prints out the value and closes.

#include <string>
#include "rclcpp/rclcpp.hpp"

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("demo");
    auto param_service = std::make_shared<rclcpp::parameter_service::ParameterService>(node);

    std::string param = "";
    auto set_parameters_results = node->set_parameters({ rclcpp::parameter::ParameterVariant("param", ""), });
    for (auto & result : set_parameters_results)
    {
        if (!result.successful)
        {
            std::cerr << "Failed to initialize parameter: " << result.reason << std::endl;
        }
    }

    while (rclcpp::ok() && param.empty())
    {
        std::cout << "Waiting for params...\n";
	rclcpp::sleep_for(std::chrono::milliseconds(1000));
	
        rclcpp::spin_some(node);
	param = node->get_parameter("param").as_string();
    }

    std::cout << "Got param: " << param << std::endl;

    return 0;
}

I use a separate node to set it, which is part of ros2: ros2param

It is not working use this rmw. The same node does work when using fastrtps. The ros2param tool just prints Failed to set parameter.

When using fastrtps for the server, and using coredx for the ros2param tool, the server prints out a bunch of these error messages:
[RTPS_MSG_IN Error] (ID:140484578068224) Bad encapsulation for KeyHash and status parameter list -> Function proc_Submsg_Data

I am using evaluation version 4.0.14 with gcc43. I have my ros2 compiled with gcc 5.4

[nate-jackson]

If it helps, here is my cmake file

cmake_minimum_required(VERSION 3.5)
project(parameter_demo)

if(MSVC)
    set(flags_for_compilers
            CMAKE_C_FLAGS
            CMAKE_CXX_FLAGS)
    foreach(flags_for_compiler ${flags_for_compilers})
        if(${${flags_for_compiler}} MATCHES "/W[0-4]")
            string(REGEX REPLACE  "/W[0-4]" "/W4" ${flags_for_compiler}
                    ${${flags_for_compiler}})
        else()
            set(${flags_for_compiler} "${${flags_for_compiler}} /W4")
        endif()
    endforeach()
  add_definitions("/arch:AVX")
elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
  set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Og -Wall -Wextra")
  set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Og -Wall -Wextra")
  add_definitions("-std=c++11")
elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    add_definitions("-std=c++11 -Weverything")
endif()

find_package(ament_cmake REQUIRED)
find_package(rcl REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rmw REQUIRED)
find_package(rmw_implementation_cmake REQUIRED)

add_executable(demo_param_server src/server.cpp)
ament_target_dependencies(demo_param_server
  "rclcpp")
add_executable(demo_param_client src/client.cpp)
ament_target_dependencies(demo_param_client
  "rclcpp")

install(TARGETS demo_param_server demo_param_client DESTINATION bin)

ament_package()

My node code is organized as follows:

Demo/
    CMakeLists.txt
    package.xml
    src/
        server.cpp
        client.cpp

client.cpp is not needed, but you can use that instead of ros2param if you want. It works the exact same way.

#include <string>
#include "rclcpp/rclcpp.hpp"

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("demo_param_client");
    auto param_service = std::make_shared<rclcpp::parameter_service::ParameterService>(node);
    auto parameters_client = std::make_shared<rclcpp::parameter_client::AsyncParametersClient>(node, "demo_param_server");

    auto var = rclcpp::parameter::ParameterVariant("param", "value");
    auto set_parameters_result = parameters_client->set_parameters({var});
    auto set_result = rclcpp::spin_until_future_complete(
      node, set_parameters_result, std::chrono::milliseconds(1000));
    if (set_result != rclcpp::executor::FutureReturnCode::SUCCESS)
    {
        fprintf(stderr, "Failed to set parameter\n");
        return 1;
    }
    auto result = set_parameters_result.get().at(0);
    if (!result.successful)
    {
        fprintf(stderr, "Error setting parameter: %s\n", result.reason.c_str());
    }
    
    return 0;
}

And here is package.xml

<package format="2">
    <name>parameter_demo</name>
    <version>1.0.0</version>
    <description>
        Demo for getting/setting parameters.
    </description>
    <author>Nathan Jackson</author>
    <author>nathan.jackson@asirobots.com</author>
    <maintainer email="nathan.jackson@asirobots.com">Nathan Jackson</maintainer>
    <license>BSD</license>

    <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>

    <build_depend>rclcpp</build_depend>

    <exec_depend>rclcpp</exec_depend>

  <test_depend>ament_cmake_gmock</test_depend>
  <test_depend>ament_cmake_gtest</test_depend>
  <test_depend>ament_cmake_nose</test_depend>  
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_cmake_cpplint</test_depend>
  <test_depend>ament_cmake_lint_cmake</test_depend>
  
  <test_depend>launch</test_depend>
  <test_depend>launch_testing</test_depend>
  <test_depend>rclcpp</test_depend>
  <test_depend>rmw_implementation</test_depend>
  <test_depend>rosidl_default_generators</test_depend>
  <test_depend>rosidl_default_runtime</test_depend>

    <export>
        <build_type>ament_cmake</build_type>
    </export>
</package>

[nate-jackson]

any update on this?

[BrannonKing]

The sample code above has an issue where the node name of the server doesn't match the remote node name for the async client. However, fixing that does not entirely solve the issue. I still have to pause (thread sleep for 1s) in between the creation of the async client and the set_parameters call (on the client) in order for the parameters to work. The other middlewares don't require a wait before sending parameters, but I might be okay with it if I knew exactly what to wait for.

[BrannonKing]

Another random error I saw while playing with this:

[rclcpp::error] take response failed for client of service 'demo_param_server__set_parameters': 
error not set, at /home/brannon/ros2_coredx_ws/src/ros2/rcl/rcl/src/rcl/client.c:210

I get this error when attempting to do my own spin-then-wait loop.

[ClarkTucker]

I suspect this is what's happening:

  parameters_client->set_parameters() 
    -> calls to send_request__() on a DDS RPC client, 
         -> ultimately causing a 'write()' on a DDS topic.  

This written sample will go unnoticed if the client and server have not completed their discovery handshake. This is equivalent to running the client when no instances of the service are running.

Does the ROS2 API provide a mechanism for a 'client' to determine if (or how many) services have been discovered? If not, then the alternative would be to retry the operation until it is successful, something like this:

    while (1)  {
      auto set_parameters_result = parameters_client->set_parameters({var});
      auto set_result = rclcpp::spin_until_future_complete(
                                                           node, set_parameters_result, std::chrono::milliseconds(1000));
      if (set_result != rclcpp::executor::FutureReturnCode::SUCCESS)
        {
          fprintf(stderr, "Failed to set parameter\n");
          //return 1;
        }
      else
        {
          auto result = set_parameters_result.get().at(0);
          if (!result.successful)
            {
              fprintf(stderr, "Error setting parameter: %s\n", result.reason.c_str());
            }
          break;
        }
    } 

Also, I believe the "take response failed" error is because the server exits before completely transmitting the response. The response message is handled asynchronously by the underlying DDS impl, and may be delayed just long enough that the application terminates before the bytes go on the network.

We could look at adding code to call 'wait_for_acks()' prior to deleting the service response datawriter, and this might help avoid that condition.

[BrannonKing]

The code you posted works for me, but if you make the timeout in spin_until_future_complete smaller, it stays in the loop forever. Also the code below stays in its do-loop forever even though the value reaches the far node successfully.

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("demo_param_client");
    auto param_service = std::make_shared<rclcpp::parameter_service::ParameterService>(node);
    auto parameters_client = std::make_shared<rclcpp::parameter_client::AsyncParametersClient>(node, "demo_param_server");
    auto var = rclcpp::parameter::ParameterVariant("param", "value");
    rcl_interfaces::msg::SetParametersResult_<std::allocator<void>> result;
    do {
        auto set_parameters_result = parameters_client->set_parameters({var});
        rclcpp::spin_some(node);
        auto res = set_parameters_result.wait_for(std::chrono::milliseconds(30));
        std::cout << "res: " << (int)res << std::endl;
        if (res != std::future_status::timeout) {
            result = set_parameters_result.get().at(0);
            break;
        }
    } while(rclcpp::ok());

    while (rclcpp::ok())
    {
        rclcpp::spin_some(node);
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        if (!result.successful) {
            std::cerr << "Failed to get parameter. Result: " << result.reason << std::endl;
        }
    }
    rclcpp::shutdown();
    return 0;
}

[ClarkTucker]

Yep, I noticed that as well. This is because the wait_for() call is timing out before getting the response.

I'm not sure why the request->reply round-trip time is so large... it suggests that there is something wrong, but where?

[BrannonKing]

The bug is actually in that sample server code above. It has a one-second stall in its processing loop. Knock that down to 10ms and you get much better behavior. I'm now satisfied that this is working correctly.

[ClarkTucker]

Ah, great!

[BrannonKing]

I'm going to have to have Nate reopen this. With CoreDX middleware, the first call to set_parameters always fails. The out-of-the-box ros2param tool only makes one attempt. Either the ros2param tool needs to change or CoreDX needs to try a little harder on the first attempt to send parameters.

[nate-jackson]

We are also seeing issues a lot with setting multiple parameters, the first seems to usually eventually set, but subsequent parameters on the same node have a harder time going through.

[BrannonKing]

I've been seeing this error when parameters don't work:

src/ros2/rclcpp/rclcpp/include/rclcpp/client.hpp
183:      fprintf(stderr, "Received invalid sequence number. Ignoring...\n");

Edit: this only happens when I run two of the same node simultaneously and send parameters to that node name. (I wasn't doing that intentionally.)

I am getting quite a bit more stability now that I've consolidated some set_parameter calls. Making multiple AsyncParameterClient objects seems to make the stability in the setter go down significantly.

[BrannonKing]

I'm getting much closer to a solution on this. I'm thinking it's not the fault of the middleware. I have modified rclcpp to expose the service_is_ready on AsyncParameterClient's internal services. Waiting for that allows me to send the first item without failure.