How to get my LED blinking? (while loop breaks rosserial python node)

edit

I'm using ROS Kinetic with Ubuntu Xenial 16.04

So i've set up my RGB Leds and have it working for turning on or off depending on what is being published. Now I would like to have the Led flashing at a certain frequency, but when I add a while loop to do so, my rosserial python node goes out of sync. How can I do this then?

I have my arduino code, and a custom library for the LEDS (leds.cpp and leds.h) files. There are other operations running such as the motors, i don't know if that affects anything or not. (If not, please ignore them)

Leds.h file:

#ifndef Leds_h
#define Leds_h
#include <Arduino.h>

void initializeLeds();
void setColor(int r, int g, int b);
void flashColor(String name);

#endif

Leds.cpp file:

#include <Leds.h>
#include <Arduino.h>
#include <std_msgs/String.h>
#include "ros.h"


#define R_PIN 44
#define G_PIN 46
#define B_PIN 45

bool initialized = false;

void initializeLeds(){
    pinMode(R_PIN, OUTPUT);
    pinMode(G_PIN, OUTPUT);
    pinMode(B_PIN, OUTPUT);
    initialized = true;
}

void flashColor(String name) {
    while (name=="PreQualify") {
    setColor(0,0,1);
    }

    if (name=="GateAndSurface") {
    setColor(0,1,0);
    }

    if (name=="None") {
    setColor(0,0,0);
    }
}
void setColor(int r, int g, int b){

    if(initialized){
    switch(r) {
    case 0: digitalWrite(R_PIN, LOW);
    break;
    case 1: digitalWrite(R_PIN, HIGH);
    break;
    default:
    break;
    }
    switch(g) {
    case 0: digitalWrite(G_PIN, LOW);
    break;
    case 1: digitalWrite(G_PIN, HIGH);
    break;
    default:
    break;
    }
    switch(b) {
    case 0: digitalWrite(B_PIN, LOW);
    break;
    case 1: digitalWrite(B_PIN, HIGH);
    break;
    default:
    break;
    }

  }
}

Arduino code:

#include <Arduino.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <controller_pkg/HorizontalMotors.h>
#include <controller_pkg/VerticalMotors.h>
#include <Motors.h>
#include <Wire.h>
#include <blue_robotics.h>
#include <Servo.h>
#include <Leds.h>
#include <string.h>

//std_msgs::String color;
std_msgs::Float32 depth;
MS5837 pressureSensor;
String name;

// Callback function for left motors
void setHorizontalMotors(const controller_pkg::HorizontalMotors&  motorIntensity)
{
   setHorizontalMotorSpeed(motorIntensity.rightMotor, motorIntensity.leftMotor);
}

// Callback function for simultaneous usage of motors
void setVerticalMotors(const controller_pkg::VerticalMotors& motorIntensity)
{
    setVerticalMotorSpeed(motorIntensity.frontRight, motorIntensity.frontLeft, motorIntensity.backRight, motorIntensity.backLeft);
}

// Callback function for LED lights.
void LEDCallback(const std_msgs::String& msg) {
    name = msg.data;
    flashColor(name);
}

ros::NodeHandle nh;

// Creates all the ROS subscriber objects
ros::Subscriber<controller_pkg::VerticalMotors> subVerticalMotors("vertical_motors", setVerticalMotors);
ros::Subscriber<controller_pkg::HorizontalMotors> subHorizontalMotors("horizontal_motors", setHorizontalMotors);
ros::Subscriber<std_msgs::String> subCurrentMission("CurrentMission", &LEDCallback);

// Create the pressure sensor publisher
ros::Publisher pubToPressureSensor("depth_current", &depth);


void setup() {
  // Initializes the node and prepares for publishing and subscribing.
  nh.initNode();
  nh.advertise(pubToPressureSensor);
  nh.subscribe(subVerticalMotors);
  nh.subscribe(subCurrentMission);
  nh.subscribe(subHorizontalMotors);


  // Initializes all the motors
  initializeVerticalMotors();
  initializeHorizontalMotors("old");

  //initializes leds
  initializeLeds();

  //Initializes wire (protocol to run)
  Wire.begin();

  // If pressure sensor does not initialize, throw error

  while(!pressureSensor.init())
  {

      nh.logerror("Pressure sensor cannot initialize.");

  }

}

void loop()
{

  pressureSensor.read();

  depth.data = pressureSensor.depth()*(3.3);

  pubToPressureSensor.publish(&depth);

  nh.spinOnce();
}

I'd like to add the loop inside my Leds.cpp file, specifically on ... (more)