Ask Your Question
0

Rosserial: Multiple Publisher and Subscriber

asked 2016-04-15 19:01:25 -0500

jdiep gravatar image

updated 2016-05-04 10:31:34 -0500

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.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2016-04-15 19:26:46 -0500

ahendrix gravatar image

You have a 5-second delay in your main loop, which means that you're only calling nh.spinOnce every 5 seconds.

nh.spinOnce() is where ROS processes incoming messages and handles callbacks, so calling it once every 5 seconds means that you'll only process the incoming serial data and update the PWM values once every 5 seconds (assuming that your incoming serial buffers don't overflow in that time)

You should probably restructure your code so that you either use a small or no loop delay, and instead trigger your leak sensor check by checking against the system clock (millis()) or using a timer.

edit flag offensive delete link more
0

answered 2016-04-16 08:29:48 -0500

jdiep gravatar image

By adding ros::NodeHandle_<ArduinoHardware, 2, 2, 150, 150> nh; // from: http://answers.ros.org/question/28890/using-rosserial-for-a-atmega168arduino-based-motorcontroller/, the problem was solved :-)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-04-15 19:01:25 -0500

Seen: 2,998 times

Last updated: May 04 '16