diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8da6be9..46b9a03 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -134,6 +134,9 @@ include_directories(
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/autopilot_node.cpp)
 
+add_executable(autopilot src/autopilot.cpp)
+target_link_libraries(autopilot ${catkin_LIBRARIES})
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
diff --git a/src/autopilot.cpp b/src/autopilot.cpp
new file mode 100644
index 0000000..841dd3f
--- /dev/null
+++ b/src/autopilot.cpp
@@ -0,0 +1,93 @@
+#include <ros/ros.h>
+#include "std_msgs/String.h"
+#include <std_msgs/Float64.h>
+#include <sensor_msgs/BatteryState.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/LaserScan.h>
+
+// Parameters
+bool isCollisionDetected = false;
+bool isFinished = false;
+int angleDetection = 15;
+double distThreshold = 0.3;
+float batThreshold = 10;
+double entroThreshold = 100000000;
+
+// Function to run when receive data from the subscribed Lidar
+void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) //
+{
+	isCollisionDetected = false;
+	
+	for (int i=0;i<angleDetection;i++){
+		
+		// Get data in a cone of +-15° in front of the robot 
+		double firstVal = data->ranges[i];
+		double lastVal = data->ranges[data->ranges.size()-i-1];
+		
+		// Check if one of the data is below the threshold
+		if (lastVal <distThreshold && lastVal != 0.0){
+			isCollisionDetected = true;
+			break;
+		}
+		if (firstVal <distThreshold&& firstVal != 0.0){
+			isCollisionDetected = true;
+			break;
+		}
+	}
+	
+}
+
+void entropyCallback(const std_msgs::Float64& entropy) //??
+{
+	if (entropy.data > entroThreshold){
+		isFinished = true;
+	}
+}
+
+void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& bat)
+{
+	if (bat->voltage < batThreshold){
+		isFinished = true;
+	}
+}
+	
+	
+int main(int argc, char** argv)
+{
+	ros::init(argc,argv, "autopilot");
+	
+	// Publish to the cmd_vel topic
+	ros::NodeHandle nh;
+	ros::Publisher cmdVelPub = nh.advertise<geometry_msgs::Twist>("cmd_vel",1);
+	
+	// Subscribe to the scan topic
+	ros::Subscriber scanSub = nh.subscribe("scan",1,scanCallback); 
+	
+	// Subscribe to the entropy topic
+	ros::Subscriber entropySub = nh.subscribe("turtlebot3_slam_gmapping/entropy",1,entropyCallback); 
+	
+	// Subscribe to the battery state topic
+	ros::Subscriber batterySub = nh.subscribe("battery_state",1,batteryCallback); 
+	
+	ros::Rate loopRate(10);
+    
+	while(ros::ok())
+	{
+		geometry_msgs::Twist msg;
+		
+		
+		if (!isFinished){
+			if (isCollisionDetected){
+				msg.angular.z = 1;
+			}
+			else{
+				msg.linear.x = 1;
+			}
+		}
+		
+		cmdVelPub.publish(msg);
+		ros::spinOnce();
+		loopRate.sleep();
+	}
+	return 0;
+}