RosSerial Arduino Mega Unable to Sync Device
Hello, For a human like service robot I want to setup some 4 Led Strips with an Arduino Mega. For every Led Strip I want to have a single Subscriber with an RGBA Message so that every LED Strip can be controlled individually. So now the curios thing. At first here is my Code so Far (Not finished yet)
Include Stuff and DEFINES
Adafruit_NeoPixel pixels_left_ear(NUMPIXELS_LEFT_EAR, PIN_LEFT_EAR, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel pixels_right_ear(NUMPIXELS_RIGHT_EAR, PIN_RIGHT_EAR, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel pixels_left_arm(NUMPIXELS_LEFT_ARM, PIN_LEFT_ARM, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel pixels_right_arm(NUMPIXELS_LEFT_ARM, PIN_LEFT_ARM, NEO_GRB + NEO_KHZ800);
ros::NodeHandle nh;
struct LedStrip {
bool FLAG_MODE_CHANGED;
cmr_led::ledstrip color;
int pixel_counter;
};
LedStrip left_ear;
LedStrip right_ear;
LedStrip left_arm;
LedStrip right_arm;
void subscriberCallbackRightEar(const std_msgs::ColorRGBA& msg)
{
//Read in Parameters to struct
}
void subscriberCallbackLeftEar(const std_msgs::ColorRGBA& msg)
{
//Read in Parameters to struct
}
void subscriberCallbackRightArm(const std_msgs::ColorRGBA& msg)
{
//Read in Parameters to struct
}
void subscriberCallbackLeftArm(const std_msgs::ColorRGBA& msg)
{
//Read in Parameters to struct
}
ros::Subscriber<std_msgs::ColorRGBA> color_subscriber_left_ear("/sobi/led_left_ear", &subscriberCallbackLeftEar);
ros::Subscriber<std_msgs::ColorRGBA> color_subscriber_right_ear("/sobi/led_right_ear", &subscriberCallbackRightEar);
ros::Subscriber<std_msgs::ColorRGBA> color_subscriber_left_arm("/sobi/led_left_arm", &subscriberCallbackLeftArm);
ros::Subscriber<std_msgs::ColorRGBA> color_subscriber_right_arm("/sobi/led_right_arm",&subscriberCallbackLeftArm);
void setup() {
nh.getHardware()->setBaud(115200);
//And Setting Up and Init Subscriber, Node and Led Strips
}
void loop() {
switch (left_ear.color.m)
{
case 0:
for (int i = 0; i < NUMPIXELS_LEFT_EAR; ++i)
{
pixels_left_ear.setPixelColor(i, pixels_left_ear.Color(left_ear.color.r, left_ear.color.g, left_ear.color.b));
}
pixels_left_ear.show();
case 1:
if (left_ear.FLAG_MODE_CHANGED)
{
//set counter to start running light at start
left_ear.pixel_counter = 0;
//clear so that there are no old colors to see
pixels_left_ear.clear();
//Change Flag back
left_ear.FLAG_MODE_CHANGED = false;
}
pixels_left_ear.setPixelColor(left_ear.pixel_counter, pixels_left_ear.Color(left_ear.color.r, left_ear.color.g, left_ear.color.b));
pixels_left_ear.show();
if (left_ear.pixel_counter == NUMPIXELS_LEFT_EAR)
{
left_ear.pixel_counter = 0;
delay(100);
}
else
{
left_ear.pixel_counter++;
}
default:
break;
}
switch (right_ear.color.m)
{
case 0:
for (int i = 0; i < NUMPIXELS_RIGHT_EAR; ++i)
{
pixels_right_ear.setPixelColor(i, pixels_right_ear.Color(right_ear.color.r, right_ear.color.g, right_ear.color.b));
}
pixels_right_ear.show();
default:
break;
}
switch (left_arm.color.m)
{
case 0:
for (int i = 0; i < NUMPIXELS_LEFT_ARM; ++i)
{
pixels_left_arm.setPixelColor(i, pixels_left_arm.Color(left_arm.color.r, left_arm.color.g, left_arm.color.b));
}
pixels_left_arm.show();
default:
break;
}
switch (right_arm.color.m)
{
case 0:
for (int i = 0; i < NUMPIXELS_RIGHT_ARM; ++i)
{
pixels_right_arm.setPixelColor(i, pixels_right_arm.Color(right_arm.color.r, right_arm.color.g, right_arm.color.b));
}
pixels_right_arm.show();
default:
break;
}
nh.spinOnce();
delay(1);
}
So I know that the buffer size of the subscriber is only 512 Bytes but theres no problem because one message is an object of four float32 variables. If I run this on the arduino and if I am starting the serial node with
rosrun rosserialpython serialnode.py /dev/ttyACM0 _baud:=115200
I am getting the error message: Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
But: If I am commenting out the last Switch/Case Query in my loop everything works well. Why is obviously the program length in my loop the problem, even though it has nothing to do with the ros stuff...? What can I do to fix it? Increasing Buffer Size doesn´t work.
I hope for fast reply and have a nice weekend. Timo
Asked by Timmo1995 on 2019-08-09 01:26:21 UTC
Answers
Hello, I ran without specifying baudrate in both Arduino IDE and terminal, the default is 57600. Then it works.
Asked by JqShun on 2019-10-30 06:34:14 UTC
Comments
Is it possible you have an uninitialised variable in the switch/case which is breaking the program?
Asked by stevejp on 2019-08-12 09:08:36 UTC
Adafruit Neopixel library uses busy waits so the calls are blocking. Maybe the loop takes too long and the rosserial link times out before the spinOnce. Maybe you can try to add a spinOnce between the two switch statements.
Asked by rreignier on 2019-09-13 02:02:18 UTC