Arduino (rosserial) node hangs during I2C reading + PWM

edit

I've been trying to troubleshoot a strange problem for a few days now. I've got a ROS Kinetic node running on a Raspberry Pi 3 (Ubuntu Mate). I'm utilizing the rosserial_arduino package to handle communication between the Arduino and my master node. Current setup is basically an Arduino Mega 2560 with a SEED Studio Motor Shield powering 2 wheels, plus an ITG3205/ADXL345/HMC5883L 9-DOF chip. I currently have two publishers set up - one outputting twistStamped messages and one chatter topic outputting loop iterations and debug information. I've got one subscriber set up receiving an int16 message from a homespun teleop script - basically listening for keys 2,4,5,6,8.

Everything works perfectly if there is no power to the motor shield (I'm using an external 6v pack). I am able to echo out twist messages which are directly linked to the xyz axis on my gyro and accel. My chatter topic shows me my current loop iteration and when I send key commands I see them echoing out as well. Similarly, if the motor power is off, I still get LED signals on the shield showing the current motor states.

If I turn on power supply to the motors and send a signal other than "full stop" which is key 5, everything hangs up. it might take a few loops but it will hang within 1-4 seconds of the motor signal being sent and received. Resetting the board alleviates the issue and all my topics begin to send/receive again.

I have traced this problem down to the ITG3205 portion that performs the I2C communication (itgRead function) - specifically after the first call to Wire.endTransmission().

Any ideas? I've tried swapping out the arduino, the shield, and the 9-dof chip with no success. As long as i've got wheels are spinning, everything hangs on gyro read. I've included relevant code snippets below.

Motor Shield code. Only included forward for brevity.

void SeeedMotorV2::allStop()
{
    digitalWrite(leftmotorspeed,0);
    digitalWrite(rightmotorspeed,0);
      digitalWrite(leftmotorForward,LOW);
     digitalWrite(rightmotorBackward,LOW);
      digitalWrite(leftmotorBackward,LOW);
      digitalWrite(rightmotorForward,LOW);

}

void SeeedMotorV2::forward(int speed)
{
  allStop();
  analogWrite(leftmotorspeed,speed); //pin 10
  analogWrite(rightmotorspeed,speed);//pin 9
  digitalWrite(leftmotorBackward,LOW);//pin 13
  digitalWrite(rightmotorBackward,LOW);//pin 11
  digitalWrite(leftmotorForward,HIGH);//pin 12
  digitalWrite(rightmotorForward,HIGH);//pin 8
}

ITG3205 code snippet.

unsigned char ITG3205::itgRead(char address, char registerAddress)
{
  //This variable will hold the contents read from the i2c device.
  unsigned char data=0;

  //Send the register address to be read.
  Wire.beginTransmission(address);
  //Send the Register Address
  Wire.write(registerAddress);

  //End the communication sequence.
  Wire.endTransmission();

  //Ask the I2C device for data
  Wire.beginTransmission(address);
  Wire.requestFrom(address, 1);

  //Wait for a response from the I2C device
  if(Wire.available()){
    //Save the data sent from the I2C device
    data = Wire.read();
  }

  //End the communication sequence.
  Wire.endTransmission();

  //Return the data read during the operation
  return data;
}

//itgAddress = 0x68
//GYRO_XOUT_H = 0x1D
//GYRO_YOUT_L = 0x1DE
int ITG3205::readX(void)
{
  int data=0;
  data = itgRead ...