pseudo-autopilot code added

src/navigation/CMakeLists.txt

@@ -134,7 +134,8 @@ include_directories(
 ## Declare a C++ executable
 ## 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)
+target_link_libraries(navigation ${catkin_LIBRARIES})
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the

src/navigation/src/navigation.cpp

@@ -1 +1,51 @@
-//Include libraries
+#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();
+    }
+}