Reputation: 13
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