autopilot_v1

src/autopilot.cpp

@@ -5,9 +5,13 @@
 #include <limits>
 #include <cmath>
 
+// Global variables to share data between the callback and main loop
+float min_range = std::numeric_limits<float>::infinity();
+std::string obstacle_direction = "none";
+
 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
 {
-    float min_range = std::numeric_limits<float>::infinity();
+    min_range = std::numeric_limits<float>::infinity();
     int min_index = -1;
 
     // Find the minimum valid range
@@ -33,34 +37,33 @@ void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
             angle += 2 * M_PI;
 
         // Determine the direction
-        std::string direction;
-
         if (angle >= -M_PI / 4 && angle <= M_PI / 4)
         {
-            direction = "front";
+            obstacle_direction = "front";
         }
         else if (angle > M_PI / 4 && angle <= 3 * M_PI / 4)
         {
-            direction = "left";
+            obstacle_direction = "left";
         }
         else if ((angle > 3 * M_PI / 4 && angle <= M_PI) || (angle >= -M_PI && angle < -3 * M_PI / 4))
         {
-            direction = "back";
+            obstacle_direction = "back";
         }
         else if (angle >= -3 * M_PI / 4 && angle < -M_PI / 4)
         {
-            direction = "right";
+            obstacle_direction = "right";
         }
         else
         {
-            direction = "unknown";
+            obstacle_direction = "unknown";
         }
 
         // Print the obstacle information
-        ROS_INFO("Obstacle detected in %s at distance: %f meters", direction.c_str(), min_range);
+        ROS_INFO("Obstacle detected in %s at distance: %f meters", obstacle_direction.c_str(), min_range);
     }
     else
     {
+        obstacle_direction = "none";
         ROS_WARN("No valid range measurements received.");
     }
 }
@@ -73,12 +76,61 @@ int main(int argc, char **argv)
     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);
+    ros::Rate rate(10); // Loop at 10 Hz
 
     while (ros::ok())
     {
-        // For this example, we're only printing obstacle directions
-        // You can add movement logic here if needed
+        geometry_msgs::Twist vel_msg;
+
+        // Obstacle avoidance logic
+        if (obstacle_direction == "front" && min_range < 0.7)
+        {
+            // Obstacle ahead; stop and decide to turn left or right based on side obstacles
+            vel_msg.linear.x = 0.0;
+
+            // Decide which way to turn based on side obstacles
+            if (obstacle_direction == "left")
+            {
+                // If obstacle on the left, turn right
+                vel_msg.angular.z = -0.5;
+                ROS_INFO("Turning right to avoid obstacle in front-left.");
+            }
+            else if (obstacle_direction == "right")
+            {
+                // If obstacle on the right, turn left
+                vel_msg.angular.z = 0.5;
+                ROS_INFO("Turning left to avoid obstacle in front-right.");
+            }
+            else
+            {
+                // If obstacle directly ahead, turn in a random direction
+                vel_msg.angular.z = 0.5; // Turn left by default
+                ROS_INFO("Turning left to avoid obstacle directly ahead.");
+            }
+        }
+        else if (obstacle_direction == "left" && min_range < 0.5)
+        {
+            // Obstacle on the left; veer right
+            vel_msg.linear.x = 0.1;
+            vel_msg.angular.z = -0.3;
+            ROS_INFO("Veering right to avoid obstacle on the left.");
+        }
+        else if (obstacle_direction == "right" && min_range < 0.5)
+        {
+            // Obstacle on the right; veer left
+            vel_msg.linear.x = 0.1;
+            vel_msg.angular.z = 0.3;
+            ROS_INFO("Veering left to avoid obstacle on the right.");
+        }
+        else
+        {
+            // Path is clear; move forward
+            vel_msg.linear.x = 0.1;
+            vel_msg.angular.z = 0.0;
+            ROS_INFO("Moving forward.");
+        }
+
+        cmd_vel_pub.publish(vel_msg);
 
         ros::spinOnce();
         rate.sleep();