General question regarding executor behavior

[liamhan0905]

Issue template

• Hardware description: Raspberry Pi 4 + Pico

It seems like mico_ros_raspberrypi_pico_sdk doesn't support multithreading. So when a new msg is received, does the executor call the subscriber and block the main, since they're both on the same thread?

I've been getting weird behaviors of the robot when using the callback. Without the callback, I can call methods like robot.moveFoward() inside the main loop and all works well. But with the subscriber, the same method will only move one of the motor.So I know it's not the hardware or the API to move the robot but something to do with the callback.

The provided code is partially implemented to keep it simple. When a Twist msg is received, it'll set the robotState instance to either RobotState::MOVE_FORWARD or RobotState::MOVE_BACKWARD. And in the main loop, it calls the method to move the robot depending on the enum.

If mico_ros_raspberrypi_pico_sdk executor does create a thread for the subscriber, then I'll have to use some sort of lock but it seems like its' on the same thread. Any thoughts??

#ifdef __cplusplus
extern "C" {
#endif

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <geometry_msgs/msg/twist.h>
#include <rmw_microros/rmw_microros.h>
#include "pico/stdlib.h"
#include "pico_uart_transports.h"

// pwm control
#include "hardware/pwm.h"
#include "hardware/gpio.h"

#ifdef __cplusplus
}
#endif
// include custom files
#include "robotControl.hpp"

#include <vector>
#include <memory>

const uint LED_PIN = 25;

rcl_publisher_t publisher;
rcl_subscription_t subscriber;
std_msgs__msg__Int32 send_msg;
std_msgs__msg__Int32 recv_msg;
geometry_msgs__msg__Twist twist_msg;

MotorControl motor1 {0,1,2};
MotorControl motor2 {3,4,5};
MotorControl motor3 {6,7,8};
MotorControl motor4 {9,10,11};
RobotControl robot {motor1, motor2, motor3, motor4};

void led_toggle(int time) {
      gpio_put(LED_PIN, 0);
      sleep_ms(time);
      gpio_put(LED_PIN, 1);
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  rcl_ret_t ret = rcl_publish(&publisher, &send_msg, NULL);
  printf("Sent: %d\n",  (int)  send_msg.data);
  send_msg.data = 100;
}

enum class RobotState {
    STOP,
    MOVE_FORWARD,
    MOVE_BACKWARD,
    TURN_RIGHT,
    TURN_LEFT
};

RobotState robotState = RobotState::STOP;

void subscription_callback(const void * msgin)
{
  const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
  auto linear_x = msg->linear.x;
  auto angular_z = msg->angular.z;
  if (linear_x > 0)
  {
    robotState = RobotState::MOVE_FORWARD;
  }
  else
  {
    robotState = RobotState::MOVE_BACKWARD;
  }
}

int main()
{
    rmw_uros_set_custom_transport(
                true,
                NULL,
                pico_serial_transport_open,
                pico_serial_transport_close,
                pico_serial_transport_write,
                pico_serial_transport_read
        );

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);

    rcl_timer_t timer;
    rcl_node_t node;
    rcl_allocator_t allocator;
    rclc_support_t support;
    rclc_executor_t executor;

    allocator = rcl_get_default_allocator();

    // Wait for agent successful ping for 2 minutes.
    const int timeout_ms = 1000;
    const uint8_t attempts = 120;

    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);

    if (ret != RCL_RET_OK)
    {
        // Unreachable agent, exiting program.
        return ret;
    }

    rclc_support_init(&support, 0, NULL, &allocator);

    // node init
    rclc_node_init_default(&node, "pico_node", "", &support);

    // create publisher
    rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "pub");

    // create subscriber
    rclc_subscription_init_default(
        &subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
        "robot/cmd_vel");

    // create timer
    rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(1000),
        timer_callback);

    send_msg.data = 0;
    recv_msg.data = 0;

    rclc_executor_init(&executor, &support.context, 2, &allocator);
    rclc_executor_add_timer(&executor, &timer);
    rclc_executor_add_subscription(&executor, &subscriber, &recv_msg, &subscription_callback, ON_NEW_DATA);

    gpio_put(LED_PIN, 1);
    while (true)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
        if (robotState == RobotState::MOVE_FORWARD)
        {
                robot.setSpeed(0.4);
                robot.moveForward();
        }
        else if (robotState == RobotState::MOVE_BACKWARD)
        {
                robot.setSpeed(0.4);
                robot.moveBackward();
        }
    }
    return 0;
}

[pablogs9]

Hello @liamhan0905 micro-ROS does not create threads, you should no block the execution on the subscription callback. Instead notify you application.

CC: @JanStaschulat

[JanStaschulat]

Hello @liamhan0905, micro-ROS does not create any threads. (neither in RCLC/Executor nor in the XRCE-DDS client middleware). You mentioned in the description, that only one motor is updated, when using Executor/Subscription. In your code snippet I see only one motor class RobotControl robot {motor1, motor2, motor3, motor4};. And that you move all motors all at once e.g. with robot.setSpeed(0.4) (I guess that the setSpeedfunction is called for every motor in that function, so I don't think that your issue has something to do with blocking times or multi-threading...)

Do you see a delay in setting the speed? Does the robot alternate between stopping and driving?

So when a new msg is received, does the executor call the subscriber and block the main, since they're both on the same thread?

This is probably a misunderstanding. What do you mean with "block"? If a new message is received, then the Executor processes the callback. In that sense the Executor "blocks" the main loop. But you could also say, that is just sequential processing of your main thread.

The executor does block the main loop in the sense of "stopping the processing of the while-loop", until a new message has been received or until the timeout (here 100ms) has expired. That is the blocking time of the executor.

The

 rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))

blocks up to 100ms if no new messages has been received. But when a new message was received, your subscription_callback is immediately called. There is no blocking time between the timepoint, at which a new message is received, and the timepoint, at which the subscription callback is processed, and the timepoint, at which the if-statement in main is processed. It all happens sequentially. :

        if (robotState == RobotState::MOVE_FORWARD)
        {
                robot.setSpeed(0.4);
                robot.moveForward();
        }
        else if (robotState == RobotState::MOVE_BACKWARD)
        {
                robot.setSpeed(0.4);
                robot.moveBackward();
        }

Instead of having this if-else-statment in main, you could also move it to the subscription_callback. And in main just use .spin().

It would help, if you could post here the code, that is working and the code that is not working - to analyze the difference.

[JanStaschulat]

Also be aware, that the endless while-loop with rclc_executor_spin_some with timeout 100ms is not processing the subscription-callback and/or the "if-else-stament in main" with a fixed period of 100ms! The spin_some function may return earlier, if a new message was received. I don't know if that was also your intend.

For a periodic control loop with motor updates, I'd propose a different setup: In addition to the subscription, create a timer with timeout 100ms. Then in the subscription callback you set RobotState. In the timer callback, the RobotState is evaluated and the motor speed and direction are assigned. With this setup, you have de-coupled asynchronous message arrival and fixed periodic control loop. In main you simply call rclc_executor_spin().

In this case, rclc is single-threaded. So you don't need any mutex/lock. But if you would run this example in multiple threads (some time in the future - different setup, ...) you would need a mutex/locks, because you would share data between the subscription callback and the timer callback.

[liamhan0905]

@JanStaschulat I like your suggestion of putting the motor control inside the timer callback. Also, the robot.setSpeed method sets the PWM speed for each 4 motors. Here are the source code if it helps (motorControl.cpp, robotControl.cpp). I confirmed that the methods (robot.moveForward() or robot.moveBackward()) work well because

Inside the while loop, if I call the robot.moveForward() or robot.moveBackward() method, all 4 motors move.

    while (true)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(500));
        robot.setSpeed(0.5);
        robot.moveForward();
        sleep_ms(2000);
        robot.moveBackward();
        sleep_ms(2000);
    }

But if I comment out the moveForward, moveBackward part from the while loop and move those into the timer_callback as shown below, I still get the similar issue of just one of the motor (motor4) spinning. Here's the full source code. I've been banging my head trying to debug the issue but hitting a wall.

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  // just setting a fixed speed for now.
  if (robotState == RobotState::MOVE_FORWARD)
  {
        robot.setSpeed(0.4);
        robot.moveForward();
  }
  else if (robotState == RobotState::MOVE_BACKWARD)
  {
        robot.setSpeed(0.4);
        robot.moveBackward();
  }
}

....

    while (true)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(500));
//      robot.setSpeed(0.5);
//      robot.moveForward();
//      sleep_ms(2000);
//      robot.moveBackward();
//      sleep_ms(2000);
    }
    return 0;

For testing, I publish

ros2 topic pub /robot/cmd_vel geometry_msgs/msg/Twist "linear:
  x: 0.4
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 

and only motor 4 spins. The subscriber subscribes to the /robot/cmd_vel topic (shown here) and then because the linear.x > 0, it sets the robotState to MOVEFORWARD. And in the timer_callback, it calls this. I would've expected this to rotate all four motors since moveForward() method worked when in the while loop.

[liamhan0905]

@JanStaschulat Also, is there a good way to debug? For example, let's say under the timer_callback, i have printf statement. But is there a way to actually  see this on the CLI?

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  rcl_ret_t ret = rcl_publish(&publisher, &send_msg, NULL);
  printf("Sent: %d\n",  (int)  send_msg.data);
  send_msg.data = 100;
}

It would be great to see the debug statement in the micro_ros_agent

1682101622.874249] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x1C4A8BEB
[1682101622.874411] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x1C4A8BEB, address: 0
[1682101622.874556] info     | Root.cpp           | create_client            | create                 | client_key: 0x702BD70F, session_id: 0x81
[1682101622.874623] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x702BD70F, address: 0
[1682101622.891873] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x702BD70F, participant_id: 0x000(1)
[1682101622.896907] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x702BD70F, topic_id: 0x000(2), participant_id: 0x000(1)
[1682101622.898953] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x702BD70F, publisher_id: 0x000(3), participant_id: 0x000(1)
[1682101622.901871] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x702BD70F, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1682101622.905862] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x702BD70F, topic_id: 0x001(2), participant_id: 0x000(1)
[1682101622.907888] info     | ProxyClient.cpp    | create_subscriber        | subscriber created     | client_key: 0x702BD70F, subscriber_id: 0x000(4), participant_id: 0x000(1)
[1682101622.910895] info     | ProxyClient.cpp    | create_datareader        | datareader created     | client_key: 0x702BD70F, datareader_id: 0x000(6), subscriber_id: 0x000(4)

[torlarse]

@liamhan0905 did you try the Raspberry Pi Debug Probe?

[torlarse]

@liamhan0905 if you are looking for debug output in the mROS agent, appending -v6 to Docker call does the trick, for example

docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble serial --dev /dev/ttyACM0 -b 115200 -v6

[torlarse]

This issue should be closed IMO. I successfully use the method prescribed by @JanStaschulat , with multiple timer callbacks, multiple subscriptions, and rclc_executor_spin(&executor);; no while() loop.

[JanStaschulat]

Great, that it is now working.