microROS - rclc add subscription does not execute always the callback
Hi community,
I am using microROS on a raspberry pi pico with ROS Dashing. I written a very small code for controlling a Stepper motor. I would like to achieve that the motor does not move when the message is null (not received message).
The method: rclc_executor_add_subscription(&executor, &sub, &msg, &sub_cb, ALWAYS);
should call always the callback and when no messages are sent, the message loaded into the subscriber function should be NULL
as documented in rcl_rclc programming. The issue is that it seems not.
The full code that I have written is:
#include <stdio.h>
#include <hardware/gpio.h>
#include <hardware/pwm.h>
#include <hardware/clocks.h>
#include <pico/binary_info.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/subscription.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int16.h>
#include <rmw_uros/options.h>
#include "pico/stdlib.h"
#include "pico_uart_transports.h"
const uint STEP_IN = 14; // Pin GPIO14
const uint DIRECTION_IN = 15; // Pin GPIO15
const uint steps_per_revolution = 200; // 360°/1.8°/step
// dichiaro qui slice come global
uint slice;
uint16_t dutycycle = 60; // 60%
// rcl_subscription_t sub
std_msgs__msg__Int16 msg;
rcl_ret_t rc;
void sub_cb(const void *msgin)
{
const std_msgs__msg__Int16 *message = (const std_msgs__msg__Int16 *)msgin;
if (message == NULL || message->data == 0)
{
// Ferma il PWM
pwm_set_enabled(slice, false);
// Accendo il led
for (int i = 0; i < 4; i++)
{
gpio_put(PICO_DEFAULT_LED_PIN, 1);
sleep_ms(250);
gpio_put(PICO_DEFAULT_LED_PIN, 0);
sleep_ms(250);
}
}
else
{
uint32_t f_pwm = (uint32_t)(message->data);
// uint16_t dutycycle = (uint16_t)(message->data);
// TOP is u16 has a max of 65535, being 65536 cycles
uint32_t top = 1000000UL / f_pwm - 1;
pwm_set_wrap(slice, top);
// Set duty-cycle
uint16_t level = (top + 1) * dutycycle / 100 - 1;
pwm_set_chan_level(slice, 0, level);
pwm_set_enabled(slice, true);
}
}
int main()
{
// Inizializzo il transport della comunicazione
rmw_uros_set_custom_transport(
true, // Transport framing
NULL, // Argument for open functions
pico_serial_transport_open, // Open transport callback
pico_serial_transport_close, // close transport callback
pico_serial_transport_write, // write transport callback
pico_serial_transport_read); // read transport callback
gpio_init(PICO_DEFAULT_LED_PIN); // Led initializing
// Set the Pin 14 come funzione PWM
gpio_set_function(STEP_IN, GPIO_FUNC_PWM);
// Setto il pin 15 come standard input output
gpio_init(DIRECTION_IN);
// Setto i due pin come output
gpio_set_dir(DIRECTION_IN, GPIO_OUT);
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); // led
// Trova quale PWM slide è connesso al pin 14
slice = pwm_gpio_to_slice_num(STEP_IN);
// pwm_set_clkdiv_mode(slice, PWM_DIV_FREE_RUNNING);
u_int32_t f_sys = clock_get_hz(clk_sys); // frequenza del clock 125MHz
// let's arbitrarily choose to run pwm clock at 1MHz
float divider = f_sys / 1000000UL;
// impongo il clock del pwm ad 1MHz
pwm_set_clkdiv(slice, divider);
// Set no Phase correct
pwm_set_phase_correct(slice, false);
// Inizializzo oggetti ROS
// rcl_timer_t timer;
rcl_node_t node;
rcl_allocator_t allocator;
rclc_support_t support;
rclc_executor_t executor; // pointer executor
rcl_subscription_t sub = rcl_get_zero_initialized_subscription();
rcl_subscription_options_t sub_opts = rcl_subscription_get_default_options();
sub_opts.qos.depth = 0; // qos: last is best
// Inizializzo il messaggio per il subscriber
std_msgs__msg__Int16__init(&msg);
allocator = rcl_get_default_allocator();
// Aspetto il ROS agent dal PC per 2minuti
const int timeout_ms = 1000;
const uint8_t attempts = 120;
// Controllo se l'agent è attivo
rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);
if (ret != RCL_RET_OK)
{
// Agent non raggiungibile, esce.
return ret;
}
rclc_support_init(&support, 0, NULL, &allocator);
// Inizializzo il nodo:
rclc_node_init_default(&node, "stepper_node", "/pico", &support);
// Alternativa per settare il qos sui messaggi
rc = rcl_subscription_init(
&sub,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
"pico_stepper",
&sub_opts ...