Read from multiple sensors with rosserial and Arduino

edit

Hello,

I'm trying to read from two Adafruit AS7341 sensors that operate on I2C bus and I'm using a multiplexer in order to avoid problems with the device address. I'm correctly able to read and print on Serial Monitor the values coming from the sensors, but when I try to add rosserial library, then it always gets out of sync with this error:

 [ERROR] [1652179560.423345]: Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

I can't understand if it is related to the code that is time consuming and the rosserial is not able to establish a communication or if it is a problem with the code I'm using with Serial1 used for the ROS and Serial0 as debug for the serial monitor.

(Obviously, I'm correctly able to read topics under ROS if I use any other code with rosserial, I'm having the problem only with this code)

#include <Adafruit_AS7341.h>
#include "Wire.h"
#include <ros.h>

#include <std_msgs/String.h>

#define TCAADDR 0x70

class NewHardware : public ArduinoHardware
{
  public:
  NewHardware():ArduinoHardware(&Serial1, 115200){};
};

ros::NodeHandle_<NewHardware>  nh;
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";


ros::Publisher spettrometro_centrale("spettrometro_centrale", &str_msg);
ros::Publisher spettrometro_sinistro("spettrometro_sinistro", &str_msg);
ros::Publisher spettrometro_destro("spettrometro_destro", &str_msg);
uint16_t *readings_buffer;
unsigned int len;

void tcaselect(uint8_t i) {
  if (i > 7) return;

  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << i);
  Wire.endTransmission();  
}

Adafruit_AS7341 as7341_0;
Adafruit_AS7341 as7341_1;
Adafruit_AS7341 as7341_2;

void setup() {
    Serial.begin(115200);
    Wire.begin();
  // Wait for communication with the host computer serial monitor

    tcaselect(0);
  if (!as7341_0.begin()){
    //Serial.println("Could not find AS7341 - LEFT");

  }
      tcaselect(0);
  as7341_0.setATIME(50);
  as7341_0.setASTEP(50);
  as7341_0.setGain(AS7341_GAIN_256X);
  as7341_0.delayForData(1);


  tcaselect(1);
    if (!as7341_1.begin()){
    //Serial.println("Could not find AS7341 - CENTER");

  }
  as7341_1.setATIME(50);
  as7341_1.setASTEP(50);
  as7341_1.setGain(AS7341_GAIN_256X);
  as7341_0.delayForData(1);

   nh.initNode();
  nh.advertise(chatter);

}

void loop() {
  // Read all channels at the same time and store in as7341 object
  String sinistro;
  String centrale;
  String destro;

  int num1 = random(150, 300);
  int num2 = random(150, 300);
  int num3 = random(150, 300);
  int num4 = random(150, 300);
  int num5 = random(150, 300);
  int num6 = random(150, 300);
  int num7 = random(150, 300);
  int num8 = random(150, 300);


  tcaselect(0);
  as7341_0.startReading();



  Serial.println("Sensor 0");
//   Print out the stored values for each channel
  Serial.print("F1 410nm : ");
  Serial.println(as7341_0.getChannel(AS7341_CHANNEL_415nm_F1));
  Serial.print("F2 435nm : ");
  int f2 = as7341_0.getChannel(AS7341_CHANNEL_415nm_F1) + num1;
  Serial.println(f2);  
  Serial.print("F3 445nm : ");
  Serial.println(as7341_0.getChannel(AS7341_CHANNEL_445nm_F2));
  Serial.print("F4 460nm : ");
  int f3 = as7341_0.getChannel(AS7341_CHANNEL_445nm_F2) + num2;
  Serial.println(f3);  
  Serial.print("F5 480nm : ");
  Serial.println(as7341_0.getChannel(AS7341_CHANNEL_480nm_F3));
  Serial.print("F6 515nm : ");
  Serial.println(as7341_0.getChannel(AS7341_CHANNEL_515nm_F4));
  Serial.print("F7 535nm : ");
  int f4 = as7341_0.getChannel(AS7341_CHANNEL_515nm_F4) + num3;
  Serial.println(f4); 
  Serial.print("F8 555nm : ");
  Serial.println(as7341_0.getChannel(AS7341_CHANNEL_555nm_F5));
  Serial.print("F9 590nm : ");
  Serial.println(as7341_0.getChannel(AS7341_CHANNEL_590nm_F6));
  Serial.print("F10 610nm : ");
  int f5 = as7341_0 ...