0
$\begingroup$

Rosanswers logo

Hey! I have been testing the rosserial_arduino (http://wiki.ros.org/rosserial) in order to run a ROS node on arduino. I tested some of the examples and thought I had the hang of it. (In case it's useful: I'm running Ubuntu 14.04 on a macbook pro).

However I'm unable to get the arduino node to publish information from the IMU connected to the arduino UNO. The IMU is the MPU9150 and I'm using an implementation of the I2Cdev library found here: https://github.com/richards-tech/MPU9150Lib (I have tried a different library, in order to understand if the problem was related to this specific library, but ended up with the same problem). If I only use the MPU9150 library, that is, if I don't try to use resserial, I'm able to get the IMU data on the arduino and print it on the serial monitor. However, if I try to use rosserial I'm unable to make the node work.

When I run the serial_node from rosserial I get the following output:

nmirod@nmirod:/$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM1 _baud:=57600
[INFO] [WallTime: 1422735802.267338] ROS Serial Python Node
[INFO] [WallTime: 1422735802.270908] Connecting to /dev/ttyACM1 at 57600 baud
/home/nmirod/catkin_ws/src/rosserial/rosserial_python/src/rosserial_python/SerialClient.py:336: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)
[ERROR] [WallTime: 1422735819.375623] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

Notice that the SyntaxWarning appears even in the tutorial examples. After some testing it seems that this problem is related to the use of the Wire library. Commenting the functions that perform the initialization and reading of the IMU, I' able to get a msg on the desired topic (although constant). However, if I try to run the node as it is, I get the "Unable to sync with device" error.

Is this problem associated with the "Wire library"/I2C communication? Can you help me out?

EDIT: Is it possible to use Serial.print()'s in combination with rosserial? When I wrote this sketch I had the caution to remove all debug prints, to be sure it wouldn't scramble the communication with ros. However, when I was out of options I decided to try to use some Serial.print()'s for debugging and it seems that using Serial.begin(57600) solved the problem (same baud rate as the node).

Althought the problem seems to be solved I still would like to understand what's going on so that if something similar happens down the road I know what to do.

EDIT2: Here is the code:

#include "ros.h"
#include "rospy_tutorials/Floats.h"

#include "freeram.h"
#include "mpu.h"
#include "I2Cdev.h"

ros::NodeHandle  nh;

float aux[] = {9, 9, 9, 9};
rospy_tutorials::Floats msg;

ros::Publisher IMUdata("IMUdata", &msg);

int ret;

void setup()
{ 
    Serial.begin(57600);  
    Fastwire::setup(400,0);
    ret = mympu_open(200);
    msg.data_length = 4;

    nh.initNode();
    nh.advertise(IMUdata);
}

void loop()
{ 
    ret = mympu_update();
    if(ret == 0){
         aux[0] = mympu.ypr[0];
         aux[1] = mympu.ypr[1];
         aux[2] = mympu.ypr[2];
    }
    aux[3] = ret;

    msg.data = aux;
 
    IMUdata.publish( &msg );
    nh.spinOnce();
}

I still want to add a subscriber to this code, but even thought he works correctly (after the addition of the Serial.begin()), once I have both the subscriber and publisher working I can't get correct data from the IMU, that is, the parameter ret comes equal to -1, what means there was some problem reading the IMU data.

The subscriber will receive an array of floats and control 2 motors throught the use of a H-bridge.

EDIT3: First, since the previously stated library was really big I switched to the other one I had tried: https://github.com/rpicopter/ArduinoMotionSensorExample

From what I can tell it seems that there is some problem when trying to update the data from the dmp of the MPU9150. the mympu_update() is returning "-1" and from what I see it seems that this happens if the dmp has any problem with it's initialization. And that problem only happens when I add the publisher. Another strange thing I noticed is that even if I don't subscribe to the topic I want (that is if I don't do nh.subscribe(sub)), but I still instantiate the subscriber, the same error happens. However if I comment/delete both the subscription to the topic and the instantiation of the subscriber, I'm able to get the data.

The code I'm using right now is:

#include <ros.h>
#include <rospy_tutorials/Floats.h>
#include <std_msgs/Empty.h>

#include "freeram.h"
#include "mpu.h"
#include "I2Cdev.h"
//#include "Wire.h"


//PORTS ASSIGNING
const int motor11       = 8;
const int motor12       = 7;
const int motor21       = 5;
const int motor22       = 4;
const int motorcontrol1 = 6;
const int motorcontrol2 = 11;

//MOTOR CONTROL VARIABLES
int motorflag = 0;
float motor1output = 0;
float motor2output = 0;

ros::NodeHandle  nh;



float aux[] = {9, 9, 9, 9};
rospy_tutorials::Floats msg;

ros::Publisher IMUdata("IMUdata", &msg);

int ret;

void receiveMotorControl(const rospy_tutorials::Floats& controlarray){
  motor1output = controlarray.data[0];
  motor2output = controlarray.data[1];
  motorflag = 1;   
}
ros::Subscriber<rospy_tutorials::Floats> sub("motorControl", &receiveMotorControl);

void setup()
{ 
  nh.getHardware()->setBaud(57600);
  nh.initNode();
  nh.advertise(IMUdata);
  nh.subscribe(sub);

  //Wire.begin();
  Fastwire::setup(400,0);

  ret = mympu_open(200);
  msg.data_length = 4;

  //PORT SETUP
  pinMode(motor11, OUTPUT);
  pinMode(motor12, OUTPUT);
  pinMode(motor21, OUTPUT);
  pinMode(motor22, OUTPUT);

}

void loop()
{ 
//  if(motorflag == 1){
//    motorControl();
//  }

  ret = mympu_update();

  if(ret == 0){
      aux[0] = mympu.ypr[0];
      aux[1] = mympu.ypr[1];
      aux[2] = mympu.ypr[2];
  }
  aux[3] = ret;

  msg.data = aux;

  IMUdata.publish( &msg );
  nh.spinOnce();
  delay(100);
}

void motorControl(){
  if(motorflag){
    if(motor1output >= 0){
      digitalWrite(motor11, HIGH);
      digitalWrite(motor12, LOW);
      analogWrite(motorcontrol1, motor1output);
    }
    else{
      digitalWrite(motor11, LOW);
      digitalWrite(motor12, HIGH);
      analogWrite(motorcontrol1, abs(motor1output));
    }
    if(motor2output >= 0){
      digitalWrite(motor21, HIGH);
      digitalWrite(motor22, LOW);
      analogWrite(motorcontrol2, motor2output);
    }
    else{
      digitalWrite(motor21, LOW);
      digitalWrite(motor22, HIGH);
      analogWrite(motorcontrol2, abs(motor2output));
    }
    motorflag = 0;
  }
}

And the output I get on the terminal is:

nmirod@nmirod:~$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM2
[INFO] [WallTime: 1422913940.904599] ROS Serial Python Node
[INFO] [WallTime: 1422913940.912298] Connecting to /dev/ttyACM2 at 57600 baud
/home/nmirod/catkin_ws/src/rosserial/rosserial_python/src/rosserial_python/SerialClient.py:336: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray, queue_size=None)
/home/nmirod/catkin_ws/src/rosserial/rosserial_python/src/rosserial_python/SerialClient.py:101: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  self.publisher = rospy.Publisher(self.topic, self.message)
[INFO] [WallTime: 1422913943.242449] Note: publish buffer size is 280 bytes
[INFO] [WallTime: 1422913943.242726] Setup publisher on IMUdata [rospy_tutorials/Floats]
[INFO] [WallTime: 1422913943.251476] Note: subscribe buffer size is 280 bytes
[INFO] [WallTime: 1422913943.251721] Setup subscriber on motorControl [rospy_tutorials/Floats]

Originally posted by nvoltex on ROS Answers with karma: 131 on 2015-01-31

Post score: 3

Original comments

Comment by tonybaltovski on 2015-01-31:
I've used the i2c library without any isseus. Try publishing slower on the arduino.

Comment by nvoltex on 2015-02-01:
Could you check my edit? I still don't understand what's happening.

Comment by tonybaltovski on 2015-02-01:
Can you post your code? Did you try slowing down the publishing rate?

Comment by nvoltex on 2015-02-02:
I have added the code, but note that it's the code before adding the subscriber. By slowing down the publishing rate are you talking about the baudrate used on the node? (thanks for helping!)

Comment by tonybaltovski on 2015-02-02:
I meant adding delay(15); or publishing at a certain rate. You maybe overfilling the serial buffer.

Comment by nvoltex on 2015-02-02:
At the time I tried using different delays however it didn't help. With the addition of Serial.begin() it started working, althought I don't know why that would help. However when I try to add a subscriber to the node, the IMU stops responding (I think he fails to initialize).

Comment by tonybaltovski on 2015-02-02:
Try initializing the node before you start the i2c comm. Also, try manually setting the baud nh.getHardware()->setBaud(BAUD); before the node initializes.

Comment by nvoltex on 2015-02-02:
I tried your suggestions and still got the same problem. I made an edit with some new information and the code I'm using right now.

Comment by tonybaltovski on 2015-02-02:
Did you create the ros_lib machine that is connecting to the Arduino?

Comment by nvoltex on 2015-02-02:
I didn't understand your question. The arduino is connected to a computer running ubuntu 14.04 and I installed the ros library on the arduino IDE following the tutorial: http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup.

Comment by tonybaltovski on 2015-02-02:
After updates, you may need to remake your ros_lib. Can you run any example sketches currently?

Comment by nvoltex on 2015-02-02:
Yes I can. In fact if I simply comment the parts related to the subscriber, I'm able to get the desired information on the topic /IMUdata (as expected). What seems to be the problem is that once I instantiate the subscriber and subscribe to the desired topic I'm unable to retrieve data from the IMU.

Comment by nvoltex on 2015-02-02:
That is, the output of mympu_update() is '-1' what seems to be related to some problem with the initializaion of the MPU9150's dmp. I added the output I get on the terminal to the last edit. btw, which i2c library did you use? Wire?

Comment by tonybaltovski on 2015-02-02:
I used wire here. That output is normal for it working. Which Arduino are you using? If leonardo, add #define USE_USBCON before #include <ros.h>.

Comment by nvoltex on 2015-02-02:
I'm using an usb uno. The library i'm using for the MPU9150 was using fast-wire and since I was having problems on the configuration of the dmp I decided to switch to wire in hope of better performance. However with wire I'm unable to even connect to the node (like in the original problem).

Comment by tonybaltovski on 2015-02-03:
I think your are just trying to publish to much data at once. I increased the serial buffer on an example here but worked but there were many errors during the connection.

Comment by nvoltex on 2015-02-03:
Thanks for your repository. Thanks to it I added confirmations to the rosserial connection before starting I2C configuration. Although I wasn't able to solve the problem with your help I'm definitely closer to a solution.

Comment by tonybaltovski on 2015-02-03:
Try asking on Github. It maybe that the I2C communication is indeed interfering since it is taking too long.

Comment by nvoltex on 2015-02-04:
Already did that ;) I'm now waiting for a response from the creator. Once I solve the problem will post an answer.

Improve this question
$\endgroup$

2 Answers 2

Reset to default
0
$\begingroup$

Rosanswers logo

I was able to run this on an Uno:

#include <ros.h>
#include <rospy_tutorials/Floats.h>
#include <std_msgs/Empty.h>

//#include "freeram.h"
//#include "mpu.h"
//#include "I2Cdev.h"
//#include "Wire.h"


//PORTS ASSIGNING
const int motor11       = 8;
const int motor12       = 7;
const int motor21       = 5;
const int motor22       = 4;
const int motorcontrol1 = 6;
const int motorcontrol2 = 11;

//MOTOR CONTROL VARIABLES
int motorflag = 0;
float motor1output = 0;
float motor2output = 0;

ros::NodeHandle  nh;



float aux[] = {9, 9, 9, 9};
rospy_tutorials::Floats msg;

ros::Publisher IMUdata("IMUdata", &msg);

int ret;

void receiveMotorControl(const rospy_tutorials::Floats& controlarray){
  motor1output = controlarray.data[0];
  motor2output = controlarray.data[1];
  motorflag = 1;
  nh.loginfo("Got a callback");
}
ros::Subscriber<rospy_tutorials::Floats> sub("motorControl", &receiveMotorControl);

void setup()
{ 
  nh.getHardware()->setBaud(57600);
  nh.initNode();
  nh.advertise(IMUdata);
  nh.subscribe(sub);

  //Wire.begin();
  //Fastwire::setup(400,0);

  //ret = mympu_open(200);
  msg.data_length = 4;

  //PORT SETUP
  pinMode(motor11, OUTPUT);
  pinMode(motor12, OUTPUT);
  pinMode(motor21, OUTPUT);
  pinMode(motor22, OUTPUT);
   while (!nh.connected()) 
  {
    nh.spinOnce();
  }
  nh.loginfo("Connected to microcontroller.");

}

void loop()
{ 
//  if(motorflag == 1){
//    motorControl();
//  }

  //ret = mympu_update();

  //if(ret == 0){
  //    aux[0] = mympu.ypr[0];
  //    aux[1] = mympu.ypr[1];
  //    aux[2] = mympu.ypr[2];
  //}
  //aux[3] = ret;

  //msg.data = aux;

  IMUdata.publish( &msg );
  nh.spinOnce();
  delay(1000);
}

void motorControl(){
  if(motorflag){
    if(motor1output >= 0){
      digitalWrite(motor11, HIGH);
      digitalWrite(motor12, LOW);
      analogWrite(motorcontrol1, motor1output);
    }
    else{
      digitalWrite(motor11, LOW);
      digitalWrite(motor12, HIGH);
      analogWrite(motorcontrol1, abs(motor1output));
    }
    if(motor2output >= 0){
      digitalWrite(motor21, HIGH);
      digitalWrite(motor22, LOW);
      analogWrite(motorcontrol2, motor2output);
    }
    else{
      digitalWrite(motor21, LOW);
      digitalWrite(motor22, HIGH);
      analogWrite(motorcontrol2, abs(motor2output));
    }
    motorflag = 0;
  }
}

Originally posted by tonybaltovski with karma: 2549 on 2015-02-02

This answer was NOT ACCEPTED on the original site

Post score: 1

Original comments

Comment by nvoltex on 2015-02-03:
That code I would be able to run as well since the problem is connected with the rosserial connection scrambling the I2C communication.

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

In the original problem I was trying to use the DMP of the MPU9150 in order to do the calculations to obtain the Yaw, Pitch and Roll directly from the MPU. However it seemed like the rosserial communication was messing up the DMP configuration and I wasn't able to make the DMP configuration to work once I tried to add both a subscriber and a publisher to the node.

The work-around I ended up using is to simply get the raw data from the IMU (magnetometer, gyro and accelerometer) and publish it to the desired topic. Then I could add a subscriber and I would still be able to get the raw data from the IMU. This raw data is then processed by a node on the PC and outputs the desired information to another topic.

Note: I would like to thanks tonybaltovski for all the help and I leave here the link to his repository where he uses rosserial in combination with I2C communication with a IMU: https://github.com/tonybaltovski/ros_arduino

Originally posted by nvoltex with karma: 131 on 2015-02-06

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by tonybaltovski on 2015-02-09:
Try declaring your node handle before you include wire.

Comment by nvoltex on 2015-02-11:
I already tried that and it didn't solve the problem. Unfortunately I wasn't able to receive an answer from the developers of the library. Tried to contact the developers of rosserial, but no response either.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.