From 6cf9a3156d5c390db6c1a2236693ea3654c55062 Mon Sep 17 00:00:00 2001
From: "paul.ewing" <paul.ewing@ecam.fr>
Date: Wed, 6 Nov 2024 15:07:33 +0100
Subject: [PATCH] bomba

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

diff --git a/src/autopilot.cpp b/src/autopilot.cpp
new file mode 100644
index 0000000..01913fd
--- /dev/null
+++ b/src/autopilot.cpp
@@ -0,0 +1,54 @@
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/LaserScan.h>
+
+bool isCollisionDetected = false;
+double scanAngle = 30;
+double distanceThreshold = 0.5;
+
+void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& data)
+{
+    // Check for obstacles in front, to be right ahead, we need to know get the center, which is at 180°, so half of the values
+    int center_index = data->ranges.size() / 2;
+	// To know the 
+    int cone_size = (data->ranges.size() * scanAngle) / 360;  // x degrees on each side
+    
+    isCollisionDetected = false;
+    for (int i = center_index - cone_size; i < center_index + cone_size; i++) {
+        if (i >= 0 && i < data->ranges.size()) {
+            if (data->ranges[i] < distanceThreshold) {  // distance threshold + verification (!std::isinf(data->ranges[i]) && !std::isnan(data->ranges[i]))
+                isCollisionDetected = true;
+                break;
+            }
+        }
+    }
+}
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "autopilot");
+    ros::NodeHandle nh;
+
+    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+    ros::Subscriber laser_sub = nh.subscribe("scan", 1, lidarCallback);
+    ros::Rate loop_rate(10);  // 10 Hz
+
+    while (ros::ok()) {
+        geometry_msgs::Twist cmd_vel;
+
+        if (isCollisionDetected) {
+            // Spin in place when collision detected
+            cmd_vel.linear = {0.0, 0.0, 0.0};
+            cmd_vel.angular = {0.0, 0.0, 2};
+        } else {
+            // Move forward when no collision
+            cmd_vel.linear = {1, 0.0, 0.0};
+            cmd_vel.angular = {0.0, 0.0, 0.0};
+        }
+
+        cmd_vel_pub.publish(cmd_vel);
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    return 0;
+}
\ No newline at end of file