rostopic pub change subsribers on Arduino [closed]

edit

Hey guys, I'm trying to create an algorythm which will allow to change subscribers by using rostopic pub command. The code is created on Arduino Portenta and I'm using the rosserial arduino. This version is for using the serial.launch file to control the communication. Right when I want to change the subscriber by publishing different letter the system is freezed and nothing changes. When I restart the serial sometimes I get the subscriber and sometimes don't. I think there a problem with overloaded buffer on cmd_msg but I don't know how to reset it. 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);
    }