Rosserial: Multiple Publisher and Subscriber
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 :-) )
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.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.
Hello. Is there any way to have the publisher outside the void loop?