 }
 
 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);

ros::NodeHandle nh;

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


void loop()
{
  if (digitalRead(3) == LOW) {
    digitalWrite(RIGHT, LOW);
    digitalWrite(LEFT, LOW);
  }
  if (millis() > publisher_timer) {
    if (digitalRead(RIGHT)) {
      str_msg_right.data = "ON";
