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!

edit

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/Rosserial_Arduino_Library/src/ros.h

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

while the error is different when changing Arduino/libraries/Rosserial_Arduino_Library/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 ...