rosserial_client multiple publishers

edit

I have a custom implementation for an Atmega1284p using pololu-avr libraries, and get an odd error when trying to publish more than one topic from the controller. With only a single topic, the program runs fine, even with more complex custom messages.

This modified HelloWorld example simply tries to populate 2 string messages, the corresponding Hardware templated definition is also given below. The resulting error is

[INFO] [WallTime: 1358904787.097966] Setup Publisher on yell [std_msgs/String]
[ERROR] [WallTime: 1358904787.321421] Tried to publish before configured, topic id 125

Is anybody able to re-create this using either this particular library or any other custom hardware? Thanks

HelloWorld.cpp

#include "ros.h"
#include "std_msgs/String.h"

// Include C headers (ie, non C++ headers) in this block
extern "C"
{
  #include <util/delay.h>
}

// Needed for AVR to use virtual functions
extern "C" void __cxa_pure_virtual(void);
void __cxa_pure_virtual(void) {}

ros::NodeHandle nh;

std_msgs::String str_msg;
std_msgs::String str_yell_msg;
ros::Publisher chatter("chatter", &str_msg);
ros::Publisher yell("yell", &str_yell_msg);

char hello[13] = "hello world!";

char hello_yell[13] = "HELLO WORLD!";

int main()
{
  uint32_t lasttime = 0UL;

  // Initialize ROS
  nh.initNode();
  nh.advertise(chatter);
  nh.advertise(yell);

  while(1)
  {
    OrangutanSerial::check();
    // Send the message every second
    if(OrangutanTime::ms() - lasttime > 1000)
    {
      str_msg.data = hello;
      chatter.publish(&str_msg);
      str_yell_msg.data = hello_yell;
      chatter.publish(&str_yell_msg);
      lasttime = OrangutanTime::ms();
    }
    nh.spinOnce();
  }

  return 0;
}

svp1284Hardware.h (templated in ros.h)

#ifndef _SVP1284_HARDWARE_H
#define _SVP1284_HARDWARE_H

extern "C"
{
  #include <pololu/orangutan.h>
}

#define port USB_COMM

class svp1284Hardware {
  public:
    svp1284Hardware(): receive_buffer_position_(0) {}

    // Initialize the AVR
    void init()
    {
      OrangutanTime::ms();// If not already called, this will call private function init()
      OrangutanSerial::receiveRing(port, receive_buffer_, sizeof(receive_buffer_));
    }

    // Read a byte of data from ROS connection.
    // If no data, returns -1
    int read()
    {
      OrangutanSerial::check();
      if (OrangutanSerial::getReceivedBytes(port)!= receive_buffer_position_)
      {
        b=(unsigned char)receive_buffer_[receive_buffer_position_];
        increment_buffer_pos();
        return b;
      }
      else
      {
          return -1;
      }
    }

    // Send bytes of data to ROS connection
    void write(uint8_t* data, int length)
    {
      OrangutanSerial::send(port,(char*) data,length);
    }


    // Returns milliseconds since start of program
    unsigned long time()
    {
      return OrangutanTime::ms();
    }

  private:
    char receive_buffer_[128];
    unsigned char receive_buffer_position_;
    unsigned char b;

    void increment_buffer_pos()
    {
      // Increment receive_buffer_position, but wrap around when it gets to
      // the end of the buffer.
      if (receive_buffer_position_ == sizeof(receive_buffer_)-1)
      {
        receive_buffer_position_ = 0;
      }
      else
      {
        receive_buffer_position_++;
      }
    }

  };

#endif