 }
 
 void message_leftCb(const std_msgs::Bool& toggle_msg) {
-  if (toggle_msg.data) {
+  if (toggle_msg.data && digitalRead(3)) {
     digitalWrite(LEFT, HIGH);
   } else {
     digitalWrite(LEFT, LOW);

{
  pinMode(RIGHT, OUTPUT);
  pinMode(LEFT, OUTPUT);
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.subscribe(sub_right);

ros::NodeHandle nh;

void message_rightCb(const std_msgs::Bool& toggle_msg) {
  if (toggle_msg.data && digitalRead(3)) {
    digitalWrite(RIGHT, HIGH);
  } else {
    digitalWrite(RIGHT, LOW);
