From e75516e56abc49f9d88c006fa770966597c2c7a4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=9CSRONG?= <“ougy.srong@ecam.fr”>
Date: Thu, 7 Nov 2024 10:05:51 +0100
Subject: [PATCH] detect where is obstacles

---
 .vscode/c_cpp_properties.json | 25 +++++++++++
 .vscode/settings.json         | 10 +++++
 src/autopilot.cpp             | 79 +++++++++++++++++++++++++++++++----
 3 files changed, 105 insertions(+), 9 deletions(-)
 create mode 100644 .vscode/c_cpp_properties.json
 create mode 100644 .vscode/settings.json

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..d45de2f
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,25 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/ougy/catkin_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/ougy/catkin_ws/src/autopilot/include/**",
+        "/home/ougy/catkin_ws/src/cv_basics/include/**",
+        "/home/ougy/catkin_ws/src/ld08_driver/include/**",
+        "/home/ougy/catkin_ws/src/ros_eeng4/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..d98cab5
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/ougy/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/ougy/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}
\ No newline at end of file
diff --git a/src/autopilot.cpp b/src/autopilot.cpp
index 76ea22f..6322e05 100644
--- a/src/autopilot.cpp
+++ b/src/autopilot.cpp
@@ -1,26 +1,87 @@
-
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
+#include <sensor_msgs/LaserScan.h>
+#include <string>
+#include <limits>
+#include <cmath>
+
+void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+    float min_range = std::numeric_limits<float>::infinity();
+    int min_index = -1;
+
+    // Find the minimum valid range
+    for (size_t i = 0; i < msg->ranges.size(); ++i)
+    {
+        float range = msg->ranges[i];
+        if (std::isfinite(range) && range < min_range)
+        {
+            min_range = range;
+            min_index = i;
+        }
+    }
+
+    if (min_index >= 0)
+    {
+        // Calculate the angle corresponding to the minimum range
+        float angle = msg->angle_min + min_index * msg->angle_increment;
+
+        // Normalize angle to be between -π and π
+        if (angle > M_PI)
+            angle -= 2 * M_PI;
+        else if (angle < -M_PI)
+            angle += 2 * M_PI;
+
+        // Determine the direction
+        std::string direction;
+
+        if (angle >= -M_PI / 4 && angle <= M_PI / 4)
+        {
+            direction = "front";
+        }
+        else if (angle > M_PI / 4 && angle <= 3 * M_PI / 4)
+        {
+            direction = "left";
+        }
+        else if ((angle > 3 * M_PI / 4 && angle <= M_PI) || (angle >= -M_PI && angle < -3 * M_PI / 4))
+        {
+            direction = "back";
+        }
+        else if (angle >= -3 * M_PI / 4 && angle < -M_PI / 4)
+        {
+            direction = "right";
+        }
+        else
+        {
+            direction = "unknown";
+        }
+
+        // Print the obstacle information
+        ROS_INFO("Obstacle detected in %s at distance: %f meters", direction.c_str(), min_range);
+    }
+    else
+    {
+        ROS_WARN("No valid range measurements received.");
+    }
+}
 
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "autopilot");
     ros::NodeHandle nh;
 
-    ros::Publisher cmdvelPub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
-
-
-    ros::Rate loop_rate(10); // 10 Hz
+    ros::Subscriber scan_sub = nh.subscribe("/scan", 10, scanCallback);
+    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
 
+    ros::Rate rate(10);
 
     while (ros::ok())
     {
-    	geometry_msgs::Twist vel_msg;
-    	vel_msg.angular.z = 1.0;
-    	cmdvelPub.publish(vel_msg);
+        // For this example, we're only printing obstacle directions
+        // You can add movement logic here if needed
 
         ros::spinOnce();
-        loop_rate.sleep();
+        rate.sleep();
     }
 
     return 0;