ROSserial Arduino Subcriber function is not run after adding Publisher

edit

I use an Arduino Uno for debugging purposes of the issue below.

The function that is run for one of the subscribers (cb_drive) is not ran/called.

I have removed lines of code until the function worked once more.

The code below works. As soon as I send some (empty) message to mule_005_commands_drive, the led on pin 13 turns on.

#include <ros.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <sensor_msgs/JointState.h>

// Read commands drive message 
void cb_drive(const sensor_msgs::JointState &cmd_drive)
{
  digitalWrite(13,HIGH);
}

ros::NodeHandle  nh;

diagnostic_msgs::DiagnosticStatus diagnostics_emergency_brake;

//  Publishers
ros::Publisher mule_005_diagnostics_emergency_brake("mule_005_diagnostics_emergency_brake", &diagnostics_emergency_brake);


//  Subscribers
ros::Subscriber<sensor_msgs::JointState> mule_005_commands_drive("mule_005_commands_drive", &cb_drive);

In tab e_setup

void setup()
{
  nh.initNode();

  // Setup sensor feedback publisher
  nh.advertise(mule_005_diagnostics_emergency_brake);

  //Setup subscribers
  nh.subscribe(mule_005_commands_drive);

  pinMode(13, OUTPUT);
}

In tab f_loop

void loop()
{

  diagnostics_emergency_brake.level = byte(0);
  diagnostics_emergency_brake.name = "Safety PLC status";
  diagnostics_emergency_brake.message = "Ok";
  diagnostics_emergency_brake.hardware_id = "Siemens Logo Safety PLC ";

  mule_005_diagnostics_emergency_brake.publish(&diagnostics_emergency_brake);

  nh.spinOnce();
}

If I add another publisher, the led stays off when I send a command to mule_005_commands_drive. I believe that this implies that the code within cb_drive is not run. Can someone help me out? I don't understand what causes this behaviour.

#include <ros.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <sensor_msgs/JointState.h>

// Read commands drive message 
void cb_drive(const sensor_msgs::JointState &cmd_drive)
{
  digitalWrite(13,HIGH);
}

ros::NodeHandle  nh;

diagnostic_msgs::DiagnosticStatus diagnostics_emergency_brake;
diagnostic_msgs::DiagnosticStatus diagnostics_parking_brake;

//  Publishers
ros::Publisher mule_005_diagnostics_emergency_brake("mule_005_diagnostics_emergency_brake", &diagnostics_emergency_brake);
ros::Publisher mule_005_diagnostics_parking_brake("mule_005_diagnostics_parking_brake", &diagnostics_parking_brake);

//  Subscribers
ros::Subscriber<sensor_msgs::JointState> mule_005_commands_drive("mule_005_commands_drive", &cb_drive);

In tab e_setup

void setup()
{
  nh.initNode();

  // Setup sensor feedback publisher
  nh.advertise(mule_005_diagnostics_emergency_brake);
  nh.advertise(mule_005_diagnostics_parking_brake);

  //Setup subscribers
  nh.subscribe(mule_005_commands_drive);

  pinMode(13, OUTPUT);
}

In tab f_loop

void loop()
{

  diagnostics_emergency_brake.level = byte(0);
  diagnostics_emergency_brake.name = "Safety PLC status";
  diagnostics_emergency_brake.message = "Ok";
  diagnostics_emergency_brake.hardware_id = "Siemens Logo Safety PLC ";

  mule_005_diagnostics_emergency_brake.publish(&diagnostics_emergency_brake);

  diagnostics_parking_brake.level = byte(0);
  diagnostics_parking_brake.name = "Motor Brake status";
  diagnostics_parking_brake.message = "Brake status unknown";
  diagnostics_parking_brake.hardware_id = "DC Motor brakes";

  mule_005_diagnostics_parking_brake.publish(&diagnostics_parking_brake);  

  nh.spinOnce();
}