Unable to call the /set_parameter service on ROS2 server from the client running on micro-ROS

[vecna-ad]

• Hardware description: Custom STM32 based board
• RTOS: Zephyr

I am running a parameter server on a /state_machine node on ROS2 computer. This parameter server gives access to several services such as:

/state_machine/describe_parameters
/state_machine/get_parameter_types
/state_machine/get_parameters
/state_machine/list_parameters
/state_machine/set_parameters
/state_machine/set_parameters_atomically

I am running a service client on micro-ros side to set-up the parameters. When I send a blank request message, the callback function is getting called. However, when I send a non-empty request message then callback is not getting called. Also, RCCHEECK function returns a failed status in that case.

I had a similar issue in the past, when I was trying to get the parameters instead of setting them. I resolved that issue by doing memory allocation in the response message. However, that trick doesn't seem to work here.

Please see the code below and let me know if you can provide some guidance to debug.

// Omitting the header files (I have too many)

// First define the globals

// Define client object
rcl_client_t client;

// Service variables
rcl_interfaces__srv__SetParameters_Response res;
rcl_interfaces__srv__SetParameters_Request req;

// allocate the memory for response message 
static rcl_interfaces__msg__SetParametersResult param_buff[10];

// Callback for the service client
void client_callback(const void * msg){
  rcl_interfaces__srv__SetParameters_Response * msgin = (rcl_interfaces__srv__SetParameters_Response * ) msg;
	std::cout << "client called the service" <<std::endl;
	std::cout << std::boolalpha;
 	std::cout << "Received service response : "  <<  msgin->results.data->successful << std::endl; 
}

int64_t seq;

//-----------------------------------------------------------------------------
void timerCallback(rcl_timer_t *timer, int64_t last_call_time)
{
	RCLC_UNUSED(last_call_time);
	static int64_t count = 0;
	if (timer != NULL) 	{
		std::cout << "count = " << count << std::endl;
		req.parameters.data[0].value.integer_value = count;
		rcl_send_request(&client, &req, &seq);
	}
	count++;
}

//-----------------------------------------------------------------------------
// This network setup function is for normal zepyhr network setup, unrelated
// to MicroROS.
//-----------------------------------------------------------------------------
#if !defined(CONFIG_NET_CONFIG_MY_IPV4_ADDR)
#error "You need to define an IPv4 Address or enable DHCPv4!"
#endif
static void setup_ipv4(struct net_if *iface)
{
	char hr_addr[NET_IPV4_ADDR_LEN];
	struct in_addr addr;
	if (net_addr_pton(AF_INET, CONFIG_NET_CONFIG_MY_IPV4_ADDR, &addr))
	{
		LOG_ERR("Invalid address: %s", CONFIG_NET_CONFIG_MY_IPV4_ADDR);
		return;
	}
	net_if_ipv4_addr_add(iface, &addr, NET_ADDR_MANUAL, 0);
	LOG_INF("IPv4 address: %s", log_strdup(net_addr_ntop(AF_INET, &addr, hr_addr,
														 NET_IPV4_ADDR_LEN)));
}

// State machine for connection
enum states
{
	WAITING_AGENT,
	AGENT_AVAILABLE,
	AGENT_CONNECTED,
} state;

//-----------------------------------------------------------------------------
int main()
{

	LOG_INF("Starting Vecna Auto-charge Application");

	// Begin normal non-MicroROS zephyr network setup
	struct net_if *iface;
#ifdef CONFIG_NETWORK_INTERFACE_INDEX
	iface = net_if_get_by_index(CONFIG_NETWORK_INTERFACE_INDEX);
	LOG_INF("CHOOSING NETWORK INTERFACE: %d", CONFIG_NETWORK_INTERFACE_INDEX);
#else
	iface = net_if_get_default();
	LOG_INF("CHOOSING DEFAULT NETWORK INTEFACE");
#endif
	if (iface == NULL)
	{
		LOG_ERR("NET INTERFACE NULL!!! EXITING");
		return -1;
	}
	setup_ipv4(iface);
	// End normal non-MicroROS zephyr network setup

	// Begin MicroROS stuff

	// Set custom MicroROS transports
	rmw_uros_set_custom_transport(
		MICRO_ROS_FRAMING_REQUIRED,
		(void *)&default_params,
		zephyr_transport_open,
		zephyr_transport_close,
		zephyr_transport_write,
		zephyr_transport_read);
	
	
	// Init micro-ROS
	rcl_allocator_t allocator = rcl_get_default_allocator();
	rclc_support_t support;
	
	
	// state machine for connection

	state = WAITING_AGENT;

	while (state != AGENT_CONNECTED)
	{
		std::cout << "Trying to connect to MicroROS Agent" << std::endl;
		switch (state)
		{
		case WAITING_AGENT:
			state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;
			break;
		case AGENT_AVAILABLE:
			state = (rclc_support_init(&support, 0, NULL, &allocator) == RCL_RET_OK) ? AGENT_CONNECTED : WAITING_AGENT;
			break;
		default:
			break;
		}
		k_msleep(1000);
	}
	std::cout << "Connected to Agent" << std::endl;

	// create node
	rcl_node_t node;
	RCCHECK(rclc_node_init_default(&node, "kinematics_node", "", &support));

	// Creating a timer
	rcl_timer_t timer;
	const unsigned int timerTimeout = RCL_MS_TO_NS(1000);
	RCCHECK(rclc_timer_init_default(&timer, &support, timerTimeout, timerCallback));

	// create client 
	RCCHECK(rclc_client_init_default(&client, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(rcl_interfaces, srv, SetParameters), "/state_machine/set_parameters"));
	
	// Create executor
	res.results.data = &param_buff[0];
	res.results.capacity=10;
	res.results.size=10;
	rclc_executor_t executor;
	RCCHECK(rclc_executor_init(&executor, &support.context,3, &allocator));

	// Add the client to the executor
	RCCHECK(rclc_executor_add_client(&executor, &client, &res, client_callback));

	// Add the timer to executor
	RCCHECK(rclc_executor_add_timer(&executor, &timer));
	
	// populate the request message
	rcl_interfaces__srv__SetParameters_Request__init(&req);
	
	req.parameters.capacity = 100;
	req.parameters.data = (rcl_interfaces__msg__Parameter*) malloc(req.parameters.capacity * sizeof(rcl_interfaces__msg__Parameter) );
	req.parameters.size = 0;

	req.parameters.data[0].name.capacity = 100;
	req.parameters.data[0].name.data = (char*) malloc(req.parameters.data[0].name.capacity * sizeof(char));
	strcpy(req.parameters.data[0].name.data,"left_marker_id");
	req.parameters.data[0].name.size = strlen(req.parameters.data[0].name.data);	
	req.parameters.data[0].value.integer_value = (int64_t)0;
	req.parameters.size++;
	

  while (1) {		
		rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));	
		// k_sleep({1});
		// usleep(1000);
	}

	// Delete the node
	RCCHECK(rcl_node_fini(&node));
	// Delete the client
	RCCHECK(rcl_client_fini(&client, &node));
}

[pablogs9]

Parameter client is not available in micro-ROS, in any case, you can check the parameter server code to investigate on how to initialize the messages: https://github.com/ros2/rclc/blob/1c0ea5af6c1d5b3715af9a8029de2e5b03821b03/rclc_parameter/src/rclc_parameter/parameter_server.c#L412-L423

Maybe you need to init the sequences:

  rcl_interfaces__srv__SetParameters_Request__init(&req);
  rcl_interfaces__srv__SetParameters_Response__init(&res);
  rcl_interfaces__msg__Parameter__Sequence__init(
    &req.parameters,
    MAX_PARAMETERS);
  req.parameters.size = 0;
  rcl_interfaces__msg__SetParametersResult__Sequence__init(
    &res.results,
    MAX_PARAMETERS);
  res.results.size = 0;

[vecna-ad]

Parameter client is not available in micro-ROS,

I have used parameter client earlier to get the parameters and it seems to work fine. The only problem I am facing is in setting the parameters.

in any case, you can check the parameter server code to investigate on how to initialize the messages:
Parameter server in micro-ROS does not handle arrays that is the reason I am using services.

Parameter server does not handle arrays, I need to use it for arrays.

Maybe you need to init the sequences:

I will try this and update you about the progress.

[vecna-ad]

@pablogs9 where do you recommend to init the sequences? On the very top? Before adding to the executor? or somewhere else?

[pablogs9]

Before rclc_executor_add_client

[vecna-ad]

OK, seems like MAX_PARAMETERS is not defined in the headers so I have assumed it as 10. Below is the code that I tried, but doesn't seem to work.

// Create executor
rcl_interfaces__srv__SetParameters_Response__init(&res);
rcl_interfaces__msg__SetParametersResult__Sequence__init(&res.results,10);
res.results.size=0;
rclc_executor_t executor;
RCCHECK(rclc_executor_init(&executor, &support.context,3, &allocator));

// Add the client to the executor
RCCHECK(rclc_executor_add_client(&executor, &client, &res, client_callback));

// Add the timer to executor
RCCHECK(rclc_executor_add_timer(&executor, &timer));

// populate the request message
rcl_interfaces__srv__SetParameters_Request__init(&req);
rcl_interfaces__msg__Parameter__Sequence__init(&req.parameters,10);	
      req.parameters.size = 0;

req.parameters.data[0].name.capacity = 100;
req.parameters.data[0].name.data = (char*) malloc(req.parameters.data[0].name.capacity * sizeof(char));
strcpy(req.parameters.data[0].name.data,"left_marker_id");
req.parameters.data[0].name.size = strlen(req.parameters.data[0].name.data);	
req.parameters.data[0].value.integer_value = (int64_t)5;
req.parameters.size++;

[vecna-ad]

So if I comment these lines, the callback is getting called, maybe because the service response is empty:

// req.parameters.capacity = 100;
// req.parameters.data = (rcl_interfaces__msg__Parameter*) malloc(req.parameters.capacity * sizeof(rcl_interfaces__msg__Parameter) );
// req.parameters.size = 0;

// req.parameters.data[0].name.capacity = 100;
// req.parameters.data[0].name.data = (char*) malloc(req.parameters.data[0].name.capacity * sizeof(char));
// strcpy(req.parameters.data[0].name.data,"left_marker_id");
// req.parameters.data[0].name.size = strlen(req.parameters.data[0].name.data);	
// req.parameters.data[0].value.integer_value = (int64_t)5;
// req.parameters.size++;

But if I uncomment, the callback doesn't get called.

[pablogs9]

Are you setting req.parameters.data[0].value.type?

Are initializing strings in res.results.data[i].reason in order to have enough memory for receiving the response?

[vecna-ad]

I have included those changes, but still doesn't work, any clue? Please see the code below:

// memory allocation for fields in result
static bool bool_param = false;
static char res_message = '\0';

// Create executor
rcl_interfaces__srv__SetParameters_Response__init(&res);
rcl_interfaces__msg__SetParametersResult__Sequence__init(&res.results,10);
res.results.data[0].reason.capacity = 100;
res.results.data[0].reason.data = (char*) malloc(res.results.data[0].reason.capacity * sizeof(char));
res.results.data[0].reason.data = &res_message; // adding or removing this line doesn't make a difference
res.results.data[0].successful = &bool_param;
res.results.size=0;

rclc_executor_t executor;
RCCHECK(rclc_executor_init(&executor, &support.context,3, &allocator));

// Add the client to the executor
RCCHECK(rclc_executor_add_client(&executor, &client, &res, client_callback));

// populate the request message
rcl_interfaces__srv__SetParameters_Request__init(&req);
rcl_interfaces__msg__Parameter__Sequence__init(&req.parameters,10);
req.parameters.size = 0;

req.parameters.capacity = 100;
req.parameters.data = (rcl_interfaces__msg__Parameter*) malloc(req.parameters.capacity * sizeof(rcl_interfaces__msg__Parameter) );

req.parameters.data[0].name.capacity = 100;
req.parameters.data[0].name.data = (char*) malloc(req.parameters.data[0].name.capacity * sizeof(char));
strcpy(req.parameters.data[0].name.data,"left_marker_id");
req.parameters.data[0].name.size = strlen(req.parameters.data[0].name.data);	
req.parameters.data[0].value.integer_value = (int64_t)5;
req.parameters.data[0].value.type = 2;
req.parameters.size++;

[vecna-ad]

Would like to share with you, how the response for /get_parameters looks like by calling from the terminal. I have not yet tried /set_parameters from terminal which we are trying to achieve here in this issue.

[vecna-ad]

@pablogs9 would you be able to reproduce and help?

[pablogs9]

Rethink this:

static char res_message = '\0';
res.results.data[0].reason.data = (char*) malloc(res.results.data[0].reason.capacity * sizeof(char));
res.results.data[0].reason.data = &res_message; // adding or removing this line doesn't make a difference

you are allocating memory and setting the pointer to a single char. It seems that you were looking to do:

res.results.data[0].reason.data = (char*) malloc(res.results.data[0].reason.capacity * sizeof(char));
res.results.data[0].reason.data[0] = res_message;

Please ensure that every sequence inside the req and res structure is filled. For example, in the req you have req.parameters.data[i].string_value or req.parameters.data[i].double_array_value uninitialized, recheck all of them.

Also, it can be a good idea to use calloc instead of malloc for ensuring zero-initialized memory.
Also, make sure that you are using the newest version of the micro-ROS agent (if possible use docker version)

I will check tomorrow if I have some time.

[vecna-ad]

@pablogs9, I meant to say that I used two different approaches for memory initialization and none of them worked.

I thought this line of comment would indicate that :

// adding or removing this line doesn't make a difference

// Method 1
static char res_message = '\0';
res.results.data[0].reason.data = &res_message; // adding or removing this line doesn't make a difference

// Method 2
res.results.data[0].reason.data = (char*) malloc(res.results.data[0].reason.capacity * sizeof(char));

Please ensure that every sequence inside the req and res structure is filled. For example, in the req you have req.parameters.data[i].string_value or req.parameters.data[i].double_array_value uninitialized, recheck all of them.

I am not able to understand, if I initialize the memory for the parent structure then why do I need to initialize the memory for child structures?

rcl_interfaces__srv__SetParameters_Response__init(&res);
rcl_interfaces__msg__SetParametersResult__Sequence__init(&res.results,10);
rcl_interfaces__srv__SetParameters_Request__init(&req);
rcl_interfaces__msg__Parameter__Sequence__init(&req.parameters,10);

Also, why do the caller needs to fill all this? Don't you think micro-ROS functions should do all this?

I will try to use to calloc instead of malloc to see if that makes any difference.

I will check tomorrow if I have some time.

Did you figure something out?

Also, I want to know if you are looking into supporting C++ soon? It is a real pain to work with C and deal with malloc/calloc, etc.

[pablogs9]

Working and tested code:

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <rcl_interfaces/srv/set_parameters.h>
#include <rcl_interfaces/srv/set_parameters.h>
#include <rcl_interfaces/msg/parameter_type.h>

#include <stdio.h>

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

// Define client object
rcl_client_t client;

// Service variables
rcl_interfaces__srv__SetParameters_Response res;
rcl_interfaces__srv__SetParameters_Request req;

void client_callback(const void * msg){
  rcl_interfaces__srv__SetParameters_Response * msgin = (rcl_interfaces__srv__SetParameters_Response * ) msg;
  printf("Response from server: %s. Reason: %s\n",msgin->results.data[0].successful ? "true" : "false", msgin->results.data[0].reason.data);
}

int main()
{
  	rcl_allocator_t allocator = rcl_get_default_allocator();
	rclc_support_t support;

	// create init_options
	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

	// create node
	rcl_node_t node;
	RCCHECK(rclc_node_init_default(&node, "int32_subscriber_rclc", "", &support));

	// create executor
	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
	RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));

	RCCHECK(rclc_client_init_default(&client, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(rcl_interfaces, srv, SetParameters), "/parameter_node/set_parameters"));

	// Init request memory
	rcl_interfaces__srv__SetParameters_Request__init(&req);
	rcl_interfaces__msg__Parameter__Sequence__init(&req.parameters, 1);

	// Assing first parameter
	rosidl_runtime_c__String__assign(&req.parameters.data[0].name, "int_param");
	req.parameters.data[0].value.type = rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER;
	req.parameters.data[0].value.integer_value = 42;

	// Init response memory
	rcl_interfaces__srv__SetParameters_Response__init(&res);
	rcl_interfaces__msg__SetParametersResult__Sequence__init(&res.results, 1);
	rosidl_runtime_c__String__assign(&res.results.data[0].reason, "                               "); // Empty string for having enought memory

	RCCHECK(rclc_executor_add_client(&executor, &client, &res, client_callback));

	int64_t seq;
	rcl_send_request(&client, &req, &seq);

  	rclc_executor_spin(&executor);
}

I have check it against a RCLCPP node that is:

#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <string>
#include <functional>

using namespace std::chrono_literals;

class ParametersClass: public rclcpp::Node
{
  public:
    ParametersClass()
      : Node("parameter_node")
    {
      this->declare_parameter<int>("int_param", 1);
      timer_ = this->create_wall_timer(
      1000ms, std::bind(&ParametersClass::respond, this));
    }
    void respond()
    {
      this->get_parameter("int_param", int_param);
      RCLCPP_INFO(this->get_logger(), "Hello %d", int_param);
    }
  private:
    int int_param;
    rclcpp::TimerBase::SharedPtr timer_;
};

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

Regarding your questions:

I am not able to understand, if I initialize the memory for the parent structure then why do I need to initialize the memory for child structures?

Please read: https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/

Also, I want to know if you are looking into supporting C++ soon?

Check ros2/rclc#126

[vecna-ad]

@pablogs9, I tested your code, it works! Thanks for the help!