Arduino Ultrasoninc Sensor HC-SR04 with ROS publisher

ultrasonic_sensor_publisher.c

// ---------------------------------------------------------------- //
// Arduino Ultrasoninc Sensor HC-SR04 with ROS publisher
// Rewritten by Jedrzej Orbik
// Using Arduino IDE 1.8.13
// ---------------------------------------------------------------- //

#include <ros.h>
#include <std_msgs/Float32MultiArray.h>

const int SENSORS_COUNT = 2;
int STARTING_PIN = 3;
int DELAY = 1;

struct UltrasonicSensor {
  int echoPin;
  int trigPin;
};

void setupSensor(UltrasonicSensor* self) {
  pinMode(self->trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(self->echoPin, INPUT); // Sets the echoPin as an INPUT
}

float getDistanceInM(UltrasonicSensor* self) {
  unsigned long duration; // variable for the duration of sound wave travel
  float distance; // variable for the distance measurement

  digitalWrite(self->trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(self->trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(self->trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  unsigned long timeout = 23529; // distance would correspond to max range of 4 m
  duration = pulseIn(self->echoPin, HIGH, timeout);
  if (duration == 0)
    return 4.0f;
  // Calculating the distance
  distance = (float)duration * 0.00034f / 2.f; // Speed of sound wave divided by 2 (go and back)
  return distance;
}

ros::NodeHandle nh;
std_msgs::Float32MultiArray array_msg;
ros::Publisher publisher("ultrasonic_sensors", &array_msg);

UltrasonicSensor us_array[SENSORS_COUNT];
float data_to_publish[SENSORS_COUNT];
int i;

void setup() {
  nh.initNode();
  nh.advertise(publisher);
  // Select pins
  for (int i=0; i<SENSORS_COUNT; i++) {
    us_array[i].echoPin = STARTING_PIN + i*2;
    us_array[i].trigPin = STARTING_PIN + i*2 + 1;
    setupSensor(&us_array[i]);
  }
  array_msg.data_length = SENSORS_COUNT;
}

void loop() {
  for (i = 0; i<SENSORS_COUNT; i++) {
    data_to_publish[i] = getDistanceInM(&us_array[i]);
  }
  array_msg.data = data_to_publish;
  publisher.publish(&array_msg);
  delay(DELAY);
  nh.spinOnce();
}