Cannot publish message with rclc_publisher_init_best_effort

jarunyawat commented 2 weeks ago

Issue template

I am trying to read the motor encoder, calculate odometry, and send the data back to my PC at 100 Hz. Using rclc_publisher_init_default, it can publish data, but only at 13 Hz. So, I tried using rclc_publisher_init_best_effort to increase the frequency, but it didn't publish anything.

Hardware description: ubuntu 22.04
RTOS: esp32 dev board
Installation type: platformio
Version or commit hash: humble

Steps to reproduce the issue

Here my esp32 code

#include <Arduino.h>
#include <ESP32Encoder.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <micro_ros_platformio.h>
#include <nav_msgs/msg/odometry.h>

#include <rmw_microros/rmw_microros.h>

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){return false;}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
#define EXECUTE_EVERY_N_MS(MS, X)  do { \
  static volatile int64_t init = -1; \
  if (init == -1) { init = uxr_millis();} \
  if (uxr_millis() - init > MS) { X; init = uxr_millis();} \
} while (0)\

enum class states {
  WAITING_AGENT,
  AGENT_AVAILABLE,
  AGENT_CONNECTED,
  AGENT_DISCONNECTED
} state;

#define LED_PIN 13
void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

#define REUCTION_GEAR 56.0
#define TICK_PER_REVOLUTION 44.0

void timer_callback(rcl_timer_t * timer, int64_t last_call_time);

ESP32Encoder encoderLeft;
ESP32Encoder encoderRight;

rcl_publisher_t publisher;
nav_msgs__msg__Odometry msg;

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

bool createEntity(){
  // Initialize micro-ROS allocator
  allocator = rcl_get_default_allocator();
  init_options = rcl_get_zero_initialized_init_options();
  RCCHECK(rcl_init_options_init(&init_options, allocator));
  RCCHECK(rcl_init_options_set_domain_id(&init_options, 50));
  //create init_options
  RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator););
  // create node
  RCCHECK(rclc_node_init_default(&node, "ESP32_node", "", &support));
  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
    "wheel_odom"));
  // create timer,
  const unsigned int timer_timeout = 10;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));
  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));
  // Sync timeout
  const int timeout_ms = 1000;

  // Synchronize time with the agent
  RCCHECK(rmw_uros_sync_session(timeout_ms));
  return true;
}

void destroyEntity(){
  rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
  (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);

  RCSOFTCHECK(rcl_publisher_fini(&publisher, &node));
  RCSOFTCHECK(rcl_timer_fini(&timer));
  RCSOFTCHECK(rclc_executor_fini(&executor));
  RCSOFTCHECK(rcl_node_fini(&node));
  RCSOFTCHECK(rclc_support_fini(&support));
  RCSOFTCHECK(rcl_init_options_fini(&init_options));
}

void setup() {
  state = states::WAITING_AGENT;
  Serial.begin(115200);
  set_microros_serial_transports(Serial);

  ESP32Encoder::useInternalWeakPullResistors = puType::up;
  encoderLeft.attachFullQuad(33, 25);
  encoderLeft.setCount(0);
  encoderRight.attachFullQuad(36, 27);
  encoderRight.setCount(0);
  delay(2000);
}

void loop() {
  switch (state) {
    case states::WAITING_AGENT:
      EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? states::AGENT_AVAILABLE : states::WAITING_AGENT;);
      break;
    case states::AGENT_AVAILABLE:
      state = (true == createEntity()) ? states::AGENT_CONNECTED : states::WAITING_AGENT;
      if (state == states::WAITING_AGENT) {
        destroyEntity();
      };
      break;
    case states::AGENT_CONNECTED:
      EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? states::AGENT_CONNECTED : states::AGENT_DISCONNECTED;);
      if (state == states::AGENT_CONNECTED) {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));
      }
      break;
    case states::AGENT_DISCONNECTED:
      destroyEntity();
      state = states::WAITING_AGENT;
      break;
    default:
      break;
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time){
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    int delta_left_tick = encoderLeft.getCount();
    int delta_right_tick = encoderRight.getCount();
    float velo_left = (2.0 * M_PI * (float)delta_left_tick / (REUCTION_GEAR * TICK_PER_REVOLUTION))/ 0.1;
    float velo_right = (2.0 * M_PI * (float)delta_right_tick / (REUCTION_GEAR * TICK_PER_REVOLUTION))/ 0.1;
    encoderLeft.clearCount();
    encoderRight.clearCount();
    int64_t time_ns = rmw_uros_epoch_nanos();
    msg.header.stamp.sec = time_ns / 1000000000;
    msg.header.stamp.nanosec = time_ns % 1000000000;
    msg.header.frame_id.data = "odom";
    msg.child_frame_id.data = "base_link";
    msg.twist.twist.linear.x = (velo_left + velo_right) / 2.0;
    msg.twist.twist.angular.z = (velo_right - velo_left) / 0.3;
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
  }
}

And here my test subscriber code in my PC.

#include "micro_ros_sub/best_effort_subscriber.hpp"

BestEffortSubscriber::BestEffortSubscriber()
: Node("best_effort_subscriber")
{

    // Create a subscriber with the "best effort" QoS profile
    subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
        "wheel_odom", rclcpp::SensorDataQoS(),
        std::bind(&BestEffortSubscriber::topic_callback, this, std::placeholders::_1)
    );
}

void BestEffortSubscriber::topic_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
    RCLCPP_INFO(this->get_logger(), "Received message");
}

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

Expected behavior

It should receive messages with the 'best effort' setting.

Actual behavior

Subscriber node does not receive data with the 'best effort' setting on the ESP32.

hippo5329 commented 2 weeks ago

You will need to change the serial baudrate to 921600.

You may follow my wiki,

https://github.com/hippo5329/micro_ros_arduino_examples_platformio/wiki

https://github.com/hippo5329/linorobot2_hardware/wiki

jarunyawat commented 2 weeks ago

Thank you for your answer, but I changed the serial baud rate to 921600, and with rclc_publisher_init_best_effort, it still isn't working. I also included --baudrate 921600 in my micro-ROS agent terminal. So, I switched back to rclc_publisher_init_default, and the topic frequency increased to 90 Hz. In both cases, whether using rclc_publisher_init_best_effort or rclc_publisher_init_default, my micro-ROS agent outputs the same.

[1724552517.485593] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1724552517.485749] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1724552517.723667] info     | Root.cpp           | create_client            | create                 | client_key: 0x6DEC8AE8, session_id: 0x81
[1724552517.723783] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x6DEC8AE8, address: 0
[1724552517.738640] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x6DEC8AE8, participant_id: 0x000(1)
[1724552517.742239] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x6DEC8AE8, topic_id: 0x000(2), participant_id: 0x000(1)
[1724552517.744275] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x6DEC8AE8, publisher_id: 0x000(3), participant_id: 0x000(1)
[1724552518.073726] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x6DEC8AE8, datawriter_id: 0x000(5), publisher_id: 0x000(3)

But I noticed that my red built-in LED (which I think indicates serial transmission) flashes very quickly when using rclc_publisher_init_default; it almost appears to be always on. However, when using rclc_publisher_init_best_effort, it flashes very slowly, around 3 Hz.

hippo5329 commented 2 weeks ago

Please start over with the int32 publisher and re-connection examples in my first wiki. You should try to change the timer to 100Hz in these examples. You may check the publishing rate with "ros2 topic hz /topic".

hippo5329 commented 2 weeks ago

You should always start from the basic. Learning step by step.

Then you should try out the second wiki, the linorobot2_hardware. 50Hz control loop is good enough for beginner's mobile robots.

micro-ROS / micro_ros_platformio