Rosserial Arduino custom msg publisher. I am publishing custom msg (6 float64 values and header) and subscribe Vector3Stamped msg from Arduino mega2560. But is not working!
The error I get is the following (the custom publisher is publishing correctly but the usual subscriber is not working):

Creation of subscriber failed: need more than 1 value to unpack

When I have the following in Arduino/libraries/RosserialArduinoLibrary/src/ros.h

 #elif defined(__AVR_ATmega2560__) 
       typedef NodeHandle_ <ArduinoHardware, 15, 15, 512, 512> NodeHandle

while the error is different when changing Arduino/libraries/RosserialArduinoLibrary/src/ros.h to the following:

#elif defined(__AVR_ATmega2560__)
   typedef NodeHandle_<ArduinoHardware, 15, 15, 512, 512,FlashReadOutBuffer_> NodeHandle; 

The error I get in this case is the folowing (the custom publisher is not working but the usual subscriber is working)

Creation of publisher failed: need more than 1 value to unpack
Tried to publish before configured, topic id 115

My arduino code is the following:

#include <HardwareSerial.h>
#include <ODriveArduino.h>
#include <Wire.h>
#include <ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/time.h>
#include <mybot/encoderMsgStamped.h>


#define ROBOT_WIDTH (480) //MILLI METER
#define WHEEL_DIAMETER (165.1)  // MILLI METER
#define WHEEL_CIRC (518.676947108) //circumference OF THE WHEEL
#define CPR_ENCODER (90)


HardwareSerial& odrive_serial1 = Serial1;
HardwareSerial& odrive_serial2 = Serial2;
HardwareSerial& odrive_serial3 = Serial3;
//axis0(M0)-left
//axis1(M1)-right
ODriveArduino odrive1(odrive_serial1);
ODriveArduino odrive2(odrive_serial2);
ODriveArduino odrive3(odrive_serial3);

// cmd_vel variables to be received to drive with
float demandx;
float demandz;

// timers for the sub-main loop
unsigned long currentMillis;
long previousMillis = 0;    // set up timers
float loopTime = 100;  //100ms
long forward0;
long forward1;
long turn0;
long turn1;




// position and velocity variables read from the ODrive
long posfl,posfr,posml,posmr,posrl,posrr;

long posfl_old,posfr_old,posml_old,posmr_old,posrl_old,posrr_old;
long posfl_diff,posfr_diff,posml_diff,posmr_diff,posrl_diff,posrr_diff;
float posfl_mm_diff,posfr_mm_diff,posml_mm_diff,posmr_mm_diff,posrl_mm_diff,posrr_mm_diff;

ros::NodeHandle nh;

//function that will be called when receiving command from host
void handle_cmd (const geometry_msgs::Twist& cmd_vel) {

     demandx = cmd_vel.linear.x; 
     demandz = cmd_vel.angular.z;
}

ros::Subscriber<geometry_msgs::Twist> cmd_vel("/cmd_vel", handle_cmd);   //create a subscriber to ROS topic for velocity commands (will execute "handle_cmd" function when receiving data)
mybot::encoderMsgStamped pos_msg;                                //create a "pos_msg" ROS message
ros::Publisher pos_pub("encoderPos", &pos_msg);                          //create a publisher to ROS topic "encoderPos" using the "pos_msg" type


void setup() {
  nh.initNode();                            //init ROS node
  nh.getHardware()->setBaud(115200);         //set baud for ROS serial communication
  nh.subscribe(cmd_vel);                    //suscribe to ROS topic for velocity commands
  nh.advertise(pos_pub);                  //prepare to publish speed in ROS topic
  odrive_serial1.begin(115200);
  odrive_serial2.begin(115200);
  odrive_serial3.begin(115200);
  odrive1.SetVelocity(0,0);
  odrive1.SetVelocity(1,0);
  odrive2.SetVelocity(0,0);
  odrive2.SetVelocity(1,0);
  odrive3.SetVelocity(0,0);
  odrive3.SetVelocity(1,0);
  odrive1.SetPosition(0,0.0);
  odrive1.SetPosition(1,0.0);
  odrive2.SetPosition(0,0.0);
  odrive2.SetPosition(1,0.0);
  odrive3.SetPosition(0,0.0);
  odrive3.SetPosition(1,0.0);
  Serial.begin(115200);
  while (!Serial) ; 



}

//_________________________________________________________________________

void loop() {
  nh.spinOnce();
  currentMillis = millis();
  if (currentMillis - previousMillis >= loopTime) {  // run a loop every 100ms          
     previousMillis = currentMillis;          // reset the clock to time it
     float modifier_lin = 1.03;        // scaling factor because the wheels are squashy / there is wheel slip etc.
     float modifier_ang = 0.92;        // scaling factor because the wheels are squashy / there is wheel slip etc.


     forward0 = demandx * 1.927982351202 * modifier_lin ; // convert m/s into counts/s
     forward1 = demandx * 1.927982351202 * modifier_lin; // convert m/s into counts/s
     turn0 = demandz * 0.46271576428  * modifier_ang;    // convert rads/s into counts/s
     turn1 = demandz * 0.46271576428 * modifier_ang;    // convert rads/s into counts/s

     forward1 = forward1*-1;      // one motor and encoder is mounted facing the other way
     odrive1.SetVelocity(1, forward0 + turn0); //-1 the motors are on opposite direction 
     odrive1.SetVelocity(0, forward1 + turn1);
     odrive2.SetVelocity(1, forward0 + turn0); //-1 the motors are on opposite direction
     odrive2.SetVelocity(0, forward1 + turn1);
     odrive3.SetVelocity(1, forward0 + turn0); //-1 the motors are on opposite direction
     odrive3.SetVelocity(0, forward1 + turn1);

    // get positions from ODrive
    posfr = odrive3.GetPosition(1) *-1;                   
    posfl = odrive3.GetPosition(0);  

    posmr = odrive2.GetPosition(1) *-1;                   
    posml = odrive2.GetPosition(0);  

    posrr = odrive1.GetPosition(1) *-1;                   
    posrl = odrive1.GetPosition(0);  

    // work out the difference on each loop, and bookmark the old value
    posfl_diff = posfl - posfl_old;
    posfr_diff = posfr - posfr_old;            
    posfl_old = posfl;
    posfr_old = posfr;

    posml_diff = posml - posml_old;
    posmr_diff = posmr - posmr_old;            
    posml_old = posml;
    posmr_old = posmr;

    posrl_diff = posrl - posrl_old;
    posrr_diff = posrr - posrr_old;            
    posrl_old = posrl;
    posrr_old = posrr;

    // calc mm from encoder counts
    posfl_mm_diff = posfl_diff /0.1735184116;
    posfr_mm_diff = posfr_diff /0.1735184116;

    posml_mm_diff = posml_diff /0.1735184116;
    posmr_mm_diff = posmr_diff /0.1735184116;

    posrl_mm_diff = posrl_diff /0.1735184116;
    posrr_mm_diff = posrr_diff /0.1735184116;

     publishPos();  
  }
 }

void publishPos() {
  pos_msg.header.stamp = nh.now();      //timestamp for odometry data
  pos_msg.fl = posfl_mm_diff;    
  pos_msg.ml = posml_mm_diff; 
  pos_msg.rl = posrl_mm_diff; 
  pos_msg.fr = posfr_mm_diff; 
  pos_msg.mr = posmr_mm_diff; 
  pos_msg.rr = posrr_mm_diff; 
  pos_pub.publish(&pos_msg); 

}

Note: With usual publish and subscribe msgs and the following Arduino/libraries/RosserialArduinoLibrary/src/ros.h every think works fine for both publishing and subscribing:

 #elif defined(__AVR_ATmega2560__)   
 typedef NodeHandle_<ArduinoHardware, 15, 15, 512, 512,FlashReadOutBuffer_>  NodeHandle;