Gabriel Za
Gabriel Za

Reputation: 13

Micro-Ros image subscriber and publisher cant work together

I'm new to Ros and micro-ros and I'm runing micro-ros on esp32 with micro_ros_arduino library.

I want to make a subscriber and publisher that when the new image message comes then publisher will re-publish it again. But I have some problem that the message cant be re-publish. when I use ros2 topic echo /micro_ros_publisher, it shows nothing. I wish someone can show me the regular way to do this kind of task. Thank you very much! Here is my code :

#include <micro_ros_arduino.h>

#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 <sensor_msgs/msg/image.h>
#include <rosidl_runtime_c/string.h>
#include <rosidl_runtime_c/string_functions.h>


sensor_msgs__msg__Image img_msg;
rcl_subscription_t subscriber;
rcl_publisher_t publisher;

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

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

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

void subscription_callback(const void *msgin) {  
    const sensor_msgs__msg__Image *received_image = (const sensor_msgs__msg__Image *)msgin;

    if (received_image == NULL) return;

    img_msg.height = received_image->height;
    img_msg.width = received_image->width;
    img_msg.step = received_image->step;
    img_msg.is_bigendian = received_image->is_bigendian;
    rosidl_runtime_c__String__assign(&img_msg.encoding, received_image->encoding.data);

    img_msg.data.data = (uint8_t *)realloc(img_msg.data.data, received_image->data.size);
    img_msg.data.capacity = received_image->data.size;

    // Copy the image data
    memcpy(img_msg.data.data, received_image->data.data, received_image->data.size);
    img_msg.data.size = received_image->data.size;

    // Republish the image
    RCSOFTCHECK(rcl_publish(&publisher, &img_msg, NULL));
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // toggle LED as a indicator
}

void setup() {
  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  
  delay(2000);

  allocator = rcl_get_default_allocator();

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

  // Create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support));

  // Create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
    "micro_ros_subscriber"));

  // Create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
    "micro_ros_publisher"));
  
  // Create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &img_msg, &subscription_callback, ON_NEW_DATA));

  // Initialize img_msg fields
  img_msg.height = 0;
  img_msg.width = 0;
  img_msg.step = 0;
  img_msg.is_bigendian = 0;
  rosidl_runtime_c__String__init(&img_msg.encoding);
  rosidl_runtime_c__String__assign(&img_msg.encoding, "rgb8"); // Example encoding type

  img_msg.data.data = NULL;  // No initial allocation
  img_msg.data.size = 0;
  img_msg.data.capacity = 0;
}

void loop() {
  delay(100);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

I've tried publisher and it works, now I want to combine subscriber as an input image then re-publish again. I cant find tutorials for doing this kind of works.

Upvotes: 0

Views: 154

Answers (0)

Related Questions