From 27e6ab1251c3bf50d226e0e1a6f946a73be8ec96 Mon Sep 17 00:00:00 2001
From: Justin CHARMETANT <justin.charmetant@ecam.fr>
Date: Thu, 7 Nov 2024 09:03:32 +0100
Subject: [PATCH 1/3] Add the cpp file

---
 src/autopilot.cpp | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)
 create mode 100644 src/autopilot.cpp

diff --git a/src/autopilot.cpp b/src/autopilot.cpp
new file mode 100644
index 0000000..4c95b26
--- /dev/null
+++ b/src/autopilot.cpp
@@ -0,0 +1,24 @@
+#include <ros/ros.h>
+#include "std_msgs/String.h"
+#include <geometry_msgs/Twist.h>
+
+int main(int argc, char** argv)
+{
+	ros::init(argc,argv, "autopilot");
+	
+	// Subscribe to the cmd_vel topic
+	ros::NodeHandle nh;
+	ros::Publisher cmdVelPub = nh.advertise<geometry_msg::Twist>("cmd_vel",1);
+    
+	ros::Rate loopRate(10);
+    
+	while(ros::ok())
+	{
+		geometry_msg::Twist msg;
+		msg.angular.z =1;
+		mdVelPub.publish(msg);
+		ros::spinOnce();
+		loopRate.sleep();
+	}
+	return 0;
+}

From 68f7cab51c4f373aa6f8d66533858ac78667c61a Mon Sep 17 00:00:00 2001
From: Justin CHARMETANT <justin.charmetant@ecam.fr>
Date: Thu, 7 Nov 2024 09:20:24 +0100
Subject: [PATCH 2/3] Create the first autopilot version to make the robot spin

---
 CMakeLists.txt    | 3 +++
 src/autopilot.cpp | 6 +++---
 2 files changed, 6 insertions(+), 3 deletions(-)

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
index 4c95b26..89f6897 100644
--- a/src/autopilot.cpp
+++ b/src/autopilot.cpp
@@ -8,15 +8,15 @@ int main(int argc, char** argv)
 	
 	// Subscribe to the cmd_vel topic
 	ros::NodeHandle nh;
-	ros::Publisher cmdVelPub = nh.advertise<geometry_msg::Twist>("cmd_vel",1);
+	ros::Publisher cmdVelPub = nh.advertise<geometry_msgs::Twist>("cmd_vel",1);
     
 	ros::Rate loopRate(10);
     
 	while(ros::ok())
 	{
-		geometry_msg::Twist msg;
+		geometry_msgs::Twist msg;
 		msg.angular.z =1;
-		mdVelPub.publish(msg);
+		cmdVelPub.publish(msg);
 		ros::spinOnce();
 		loopRate.sleep();
 	}

From a739f4e13eeeead024b9d612a661141d6990e7e3 Mon Sep 17 00:00:00 2001
From: Justin CHARMETANT <justin.charmetant@ecam.fr>
Date: Thu, 7 Nov 2024 12:06:17 +0100
Subject: [PATCH 3/3] update the cpp file to map the environment with the SLAM
 node

---
 src/autopilot.cpp | 75 +++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 72 insertions(+), 3 deletions(-)

diff --git a/src/autopilot.cpp b/src/autopilot.cpp
index 89f6897..841dd3f 100644
--- a/src/autopilot.cpp
+++ b/src/autopilot.cpp
@@ -1,21 +1,90 @@
 #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");
 	
-	// Subscribe to the cmd_vel topic
+	// 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;
-		msg.angular.z =1;
+		
+		
+		if (!isFinished){
+			if (isCollisionDetected){
+				msg.angular.z = 1;
+			}
+			else{
+				msg.linear.x = 1;
+			}
+		}
+		
 		cmdVelPub.publish(msg);
 		ros::spinOnce();
 		loopRate.sleep();