diff --git a/RosAria.cpp b/RosAria.cpp
index d10700e..6aa801a 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -627,30 +627,33 @@ void RosAriaNode::publishRobotInfo()
     robot_info_msg.twist.angular.z = robot->getRotVel()*M_PI/180;
     robot_info_msg.state.data = robot_state;
     robot_info_msg.clutch.data = robot->areMotorsEnabled();
-    // robot_info_msg.header.stamp = ros::Time::now();
+    robot_info_msg.obstacle_detected.data = is_near_obstacle
 
     robot_info_pub.publish(robot_info_msg);
 }
 
 void RosAriaNode::publish()
 {
-    // Safety check
-    // is_near_obstacle = isCloseToObstacle();
+    // Safety distance check
+    is_near_obstacle = isCloseToObstacle();
 
-    // if (is_near_obstacle)
-    // {
-    //     robot->lock();
+    if (is_near_obstacle)
+    {
+        robot->lock();
 
-    //     robot->setVel(0);
-    //     if(robot->hasLatVel())
-    //         robot->setLatVel(0);
-    //     robot->setRotVel(0);
+        robot->setVel(0);
+        if(robot->hasLatVel())
+            robot->setLatVel(0);
+        robot->setRotVel(0);
 
-    //     robot->comInt(ArCommands::ESTOP,0); // ?
-    //     robot->unlock();
+        robot->comInt(ArCommands::ESTOP,0); // ?
+        robot->unlock();
+
+        robot_state = false;
+    }
+
+    publishRobotInfo();
 
-    //     robot_state = false;
-    // }
 
     robot->requestEncoderPackets();
     Wheel.header.stamp = ros::Time::now();
@@ -825,9 +828,6 @@ void RosAriaNode::publish()
         robot->setRotVel(0);
         // ROS_INFO("WATCHDOG %d", WATCHDOG);
     }
-
-    publishRobotInfo();
-
 }
 
 bool  RosAriaNode::gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)