Rosserial: Multiple Publisher and Subscriber

edit

Hello ROS Community,

I'm stuck with a problem and glad for any tips.

The situation is following.

My goal is to build a submersible robot ( www.scubo.ch if you are interested :-) )

1. I wrote a ROS packages that sends an array of numbers on a specific topic called pwm_signal. I programmed the Arduino UNO such that it can read this topic from the terminal and send the appropriate PWM signal to the ESC of the motors. This subscriber part works by itself and has been tested.

2. Additionally, I wrote a program on the arduino than reads from a leakage sensor and sends the signal to a ROS topic called leakage_signal. This publisher part by itself works and has been tested.

Problem: If I combine this two subscriber and publisher program into one single Arduino ino file, ROS can't deal with it at the same time. I am able to read out the leakage signal from the according ROS topic, but the thruster dont run, altough i give a signal at pwm_signal.

This is the Arduino code I'm working on:

//johann, 8. april 2016

#if defined(ARDUINO) && ARDUINO >= 100
  #include "Arduino.h"
#else
  #include <WProgram.h>
#endif

#include <Servo.h> 
//#define USE_USBCON //uncomment this when running on the UDOO
#include <ros.h>
#include <ArduinoHardware.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt16MultiArray.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

ros::NodeHandle nh; //instantiate node handle, set up the serial port communication

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); //default adress 0x40 called, adress of break-out board

std_msgs::String str_msg;

ros::Publisher chatter("leak_signal", &str_msg); //definition leakage topic, string

char dry[]="Everything is dry, keep going!";

char wet[]="Water leakage, manual abort!";

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void pwm_cb( const std_msgs::UInt16MultiArray& cmd_msg){ // callback function of thruster allocation

  for(int i=0; i<8; i++) { //runs every thrusters via for-loop

     pwm.setPWM(i,0, cmd_msg.data[i]); //PWM signal, state where it goes high, state where it goes low, 0-4095, deadband 351-362

  }

}

ros::Subscriber<std_msgs::UInt16MultiArray> sub("pwm_signal", pwm_cb); //definition PWM topic and callback function

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void setup() { // this function runs first and only one time on each upload

  nh.initNode();

  nh.advertise(chatter); // for leakage sensor, string

  nh.subscribe(sub); // for thruster allocation

  pwm.begin();

  pwm.setPWMFreq(60); //60 Hz PWM signal

  for(int j=0;j<8;j++) {  //initialize every thruster via channel (0-7) with a for-loop 

    pwm.setPWM(j,0,351);

  }

}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void loop() {
  nh.spinOnce();

  //defining the pins on the UDOO
  int s1=analogRead(A0);
  int s2=analogRead(A1);
  int s3=analogRead(A2);
  int s4=analogRead(A3);

  // transformation into voltage signal
  float v1=s1*(5.0/1023,0);
  float v2=s2*(5.0/1023,0);
  float v3=s3*(5.0/1023,0);
  float v4=s4*(5.0/1023,0);

  if(v1>3 ||v2>3||v3>3||v4>3) { // action if water leakage

   str_msg.data=wet;
   chatter.publish(&str_msg);

  }


  else { // action if no water leakage

    str_msg.data=dry;
    chatter.publish(&str_msg);

  }

  delay(1);

}

I'm glad for any help or tips.