From 8308a1487a4c51627b2dea7096253f67367f19d2 Mon Sep 17 00:00:00 2001
From: "paul.ewing" <paul.ewing@ecam.fr>
Date: Tue, 19 Nov 2024 11:45:42 +0100
Subject: [PATCH] Simultaneous navigation and SLAM

---
 src/navigation/CMakeLists.txt     |  4 +--
 src/navigation/src/navigation.cpp | 54 ++++++++++++++++++++++++++++++-
 2 files changed, 55 insertions(+), 3 deletions(-)

diff --git a/src/navigation/CMakeLists.txt b/src/navigation/CMakeLists.txt
index ef124e1..8d4d768 100644
--- a/src/navigation/CMakeLists.txt
+++ b/src/navigation/CMakeLists.txt
@@ -135,7 +135,7 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/navigation_node.cpp)
-
+add_executable(navigation src/navigation.cpp)
 ## 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
@@ -150,7 +150,7 @@ include_directories(
 # target_link_libraries(${PROJECT_NAME}_node
 #   ${catkin_LIBRARIES}
 # )
-
+target_link_libraries(navigation ${catkin_LIBRARIES})
 #############
 ## Install ##
 #############
diff --git a/src/navigation/src/navigation.cpp b/src/navigation/src/navigation.cpp
index 44b4ba4..e26b105 100644
--- a/src/navigation/src/navigation.cpp
+++ b/src/navigation/src/navigation.cpp
@@ -1 +1,53 @@
-//Include libraries
\ No newline at end of file
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/LaserScan.h>
+ 
+bool isCollisionDetected = false;
+double scanAngle = 15; 
+double distanceThreshold = 0.28; // distance in meters
+
+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) {
+            cmd_vel.linear.x = 0;
+            cmd_vel.angular.z = 1;
+        } else {
+            cmd_vel.linear.x = 0.5;
+            cmd_vel.angular.z = 0;
+        }
+        cmd_vel_pub.publish(cmd_vel);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+ 
+    return 0;
+}
\ No newline at end of file