rosservice and node conflicting on arduino

[Sree-Aslesh]

i am working on an autonomous quadrotor having a raspberrypi on board wiht ubuntu mate 16.04 and laptop on ubuntu with 16.04 as well. The arduino code is as shown.

#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <LiquidCrystal.h>
#include <Servo.h>
#include <quadrotor/arm.h>
#include <quadrotor/disarm.h>
#include <quadrotor/land.h>
#include <quadrotor/speed.h>

ros::NodeHandle  nh;
using quadrotor::arm;
using quadrotor::disarm;
using quadrotor::land;

LiquidCrystal lcd(7, 6, 5, 4, 3, 2);
Servo esc1,esc2,esc3,esc4;
int flag=0;

void arm(const arm::Request & req, arm::Response & res)
{
  flag=1;
}

void disarm(const disarm::Request & req, disarm::Response & res)
{
  flag=0;
}

void land(const land::Request & req, land::Response & res)
{
  flag=2;
}

void callback(const quadrotor::speed &msg)
{
  if(flag==1)
  {   int a,b,c,d;
     a=msg.m1.data;
     b=msg.m2.data;
     c=msg.m3.data;
     d=msg.m4.data;
     bldc_set(a,b,c,d);
  }
  else
  bldc_off();
}

ros::Subscriber<quadrotor::speed> sub("/quad/motor_speed", &callback );
ros::ServiceServer <arm::Request, arm::Response> server1("arm", &arm);
ros::ServiceServer <disarm::Request, disarm::Response> server2("disarm", &disarm);
ros::ServiceServer <land::Request, land::Response> server3("land", &land);

void bldc_off()
{
  esc1.write(20);   //ESC arm command. ESCs won't start unless input speed is less during initialization.
  esc2.write(20);
  esc3.write(20);
  esc4.write(20);
  delay(100);
}

void bldc_set(const int a, const int b, const int c, const int d)
{
  esc1.write(a);   //ESC arm command. ESCs won't start unless input speed is less during initialization.
  esc2.write(b);
  esc3.write(c);
  esc4.write(d);
  delay(100);
}

void setup()
{
  Serial.begin(11520); 

  esc1.attach(8); //Specify here the pin number on which the signal pin of ESC is connected.   // intialize esc's
  esc2.attach(9);
  esc3.attach(10);
  esc4.attach(11);

  esc1.write(40);   //ESC arm command. ESCs won't start unless input speed is less during initialization.
  esc2.write(40);
  esc3.write(40);
  esc4.write(40);

  pinMode(13, OUTPUT);

  nh.initNode();
  nh.subscribe(sub);
  nh.advertiseService(server1);
  nh.advertiseService(server2);
  nh.advertiseService(server3);

  lcd.begin(16, 2);
  lcd.print("QUADROTOR");
    delay(3000); 
}

void loop()
{

  if(flag==0)
  {

   lcd.setCursor(0, 2);
   lcd.print("Status: Disarmed");
   lcd.display();
   bldc_off();
  }
  else if(flag==1)
  {

   lcd.setCursor(0, 2);
   lcd.print("                ");
   lcd.display();
   lcd.setCursor(0, 2);
   lcd.print("Status: Armed");
  }
  else if (flag == 2 )
   {lcd.setCursor(0, 2);
   lcd.print("                ");
   lcd.display();
   lcd.setCursor(0, 2);
   lcd.print("Status: Land");
   }
  nh.spinOnce();
  delay(500);
}

I have define 3 services , arm, disarm , land . and the node is just providing speeds. I execute the commands in the following order -

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <tf/tf.h>
#include <iostream>
#include <quadrotor/speed.h>

using namespace std;

double roll, pitch, yaw;

void imu_data(const sensor_msgs::Imu msg)
{
    tf::Quaternion q(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w);
    tf::Matrix3x3 m(q);

m.getRPY(roll, pitch, yaw);

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "quad_node");
  ros::NodeHandle n;
  ros::Rate loop_rate(10);
  ros::Subscriber sub1 = n.subscribe("/mavros/imu/data", 10, imu_data);
  ros::Publisher pub_speed = n.advertise<quadrotor::speed>("/quad/motor_speed", 1000);
  quadrotor::speed speed;

  while(ros::ok())
  {
    speed.m1.data=40;
    speed.m2.data=55;
    speed.m3.data=65;
    speed.m4.data=75;
    pub_speed.publish(speed);
    ros::spinOnce();
  }
  return 0;
}

1. connect to my arduino mega to the raspberrpi using -
   rosrun rosserial_python serial_node.py _baud:=115200 _port=/dev/ttyACM1
2. I run the main node using - rosrun quadrotor quad-node
3. i call the arm service rosservice call /arm arm

and for that as soon as i call the service the terminal in which i connected my arduino it hangs and then shows this - lost sync with device .. restarting .. please help me out on this @mikepurvis @blubbi321

[kobaan]

Your arduino code has a typo: Serial.begin(11520); instead of Serial.begin(115200);

[adbidwai]

@Sree-Aslesh Was your issue solved ?

[Sree-Aslesh]

@adbidwai yes it was, it was the typo that caused the loss of sync but besides that, my ROS master was forced to run at a higher rate due to which also the problem of sync had occurred.