rosserial not working with Bluetooth and custom msg

edit

Hi guys,

I am currently running an Arduino MEGA ADK robot which is sending me data from its motors via bluetooth through an HC-05 device. This HC-05 is coupled with another HC-05 connected to an Arduino UNO, and it is THAT Arduino that sends me the data via rosserial.

The connection works normally in a normal ROS node run in the Arduino, the topic gets published efficiently.

The problem is, when I enable (by code) my arduino to receive the bluetooth data from the other arduino, the rosserial connection doesn't work. I get this error message:

dani@ROS:~/catkin_ws$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0
[INFO] [WallTime: 1462188244.111483] ROS Serial Python Node
    [INFO] [WallTime: 1462188244.122188] Connecting to /dev/ttyACM0 at 57600 baud
    [ERROR] [WallTime: 1462188261.230066] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

Things:

• Changing the Baud rate inside ArduinoHardware.h to 9600 as @PaulBouchier suggested in another question. doesn't work

• Is not a problem of the connection between both HC-05, because if I make the data to be printed in the Serial Monitor of the Arduino IDE, the data appears correctly. So now the problem should be between the conection of the UNO and the Master via rosserial.

• I am using a custom message for the things said, as appears in the #includes:

  Header header

  uint8 obstacle

  uint8 state_m1

  uint8 state_m2

  uint8 state_m3

  uint8 state_m4

  uint8 d_left

  uint8 d_right

  uint8 d_center

I post the code below: (forget about the names of topics, i just recycled the Ultrasound ros example):

#include <SoftwareSerial.h>
#include <ros.h>
#include <ros/time.h>
//#include <sensor_msgs/Range.h>
#include <pkg_2/msg_2.h>

//variables globales
char Buffer[11];
int SoF=0x01; 
int Eof=0x04;
int i=0;
int trama_ok=0;
//declaracion objetos
SoftwareSerial BT(10,11);      //Rx,Tx: pins 7,8 de la placa OBJETO BLUETOOTH

ros::NodeHandle  nh;

pkg_2::msg_2 range_msg;
ros::Publisher pub_range( "/ultrasound", &range_msg);



void setup()
{
  Serial.begin(9600);
  BT.begin(9600);
  delay(1);
  nh.initNode();
  nh.advertise(pub_range);
  delay(1);

}

void loop()
{
     if (BT.available())
  {
    int a=BT.read();
    Buffer[i]=a;
    i++;
    if (i==8) {trama_ok=1;}
    else {trama_ok=0;}

  }

  if(trama_ok==1){
         range_msg.obstacle = Buffer[1];
         range_msg.state_m1 = Buffer[2];
         range_msg.state_m2 = Buffer[3];
         range_msg.state_m3 = Buffer[4];
         range_msg.state_m4 = Buffer[5];
         range_msg.d_left = Buffer[6];
         range_msg.d_right = Buffer[7];
         range_msg.d_center = Buffer[8];
    pub_range.publish(&range_msg);
          nh.spinOnce();
  }
}

I thought that could be that rosserial doesn't work properly when also working with bluetooth...

Any thoughts?, I really need this working.

Thanks a lot.

EDIT 1: I've tried with: $ roslaunch rosserial_server serial.launch and the conection reaches the Arduino (at least the Rx led of the arduino is blinking) but anyway the topic /Ultrasound is not published.

This works succcesfully when using normal ROS sketches (NON-CUSTOM message sketches). Could be that the SoftwareSerial initialized was causing a mess? Same for the Serial.begin(9600), could that be messing with ... (more)