Rostopic pub change subcriber arduino

edit

Hey guys, I'm trying to create an algorytm which will allow to chagne subscriber by typing a rostopic pub in terminal. The code is created on Arduino Portenta and I'm using rosserial library. To allow the connection between arduino and ROS I'm using serial.launch file. Right now I have got a problem with changing the subscriber. When I publish the letter there are no changes but after I restart the launch file sometimes I got the subscriber and sometimes not. I think the problem is with string buffor maybe it is overloaded somehow. The code:

#include <ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle nh;

void messageCb( const geometry_msgs::Twist& msg) {

}
 void messageCb2( const geometry_msgs::Twist& msg) {

  }
   ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb );

    ros::Subscriber<geometry_msgs::Twist> sub2("local_nav", &messageCb2 );

      void pwm( const std_msgs::String& cmd_msg)
    {
   if (cmd_msg.data[0]=='q')
    {
        nh.subscribe(sub);
   }

        else if (cmd_msg.data[0]=='a')
   {
      nh.subscribe(sub2);
      }

    else if (cmd_msg.data[0]=='z')
      {

       }
      }
        ros::Subscriber<std_msgs::String> sub1("servo", pwm);


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

          nh.initNode();
         nh.subscribe(sub1); 
          }


         void loop() 
            { 
             nh.spinOnce();
             delay(10);
               }