ROSserial Arduino Subcriber function is not run after adding Publisher

asked 2018-10-14 12:32:33 -0500

R Valk gravatar image

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();
}
edit retag flag offensive close merge delete

Comments

Probably memory issues , try using a Arduino Mega , it might help .

chrissunny94 gravatar imagechrissunny94 ( 2018-10-15 00:08:22 -0500 )edit

I have the same issue on a Mega. The code with 1 publisher works and the one with 2 does not seem to let me run the cb_drive section. If indeed it was/is a memory issues, how would I be able to identify that?

R Valk gravatar imageR Valk ( 2018-10-15 00:34:35 -0500 )edit

It indeed seems to be a memory issue. We will try to increase the memory so that the 8kb wil hopefully not be a problem.

http://wiki.ros.org/rosserial/Overvie... Section 1.4 Arrays

R Valk gravatar imageR Valk ( 2018-10-18 03:56:32 -0500 )edit