bombav3

src/autopilot.cpp

@@ -1,18 +1,22 @@
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 #include <sensor_msgs/LaserScan.h>
-
+#include <sensor_msgs/BatteryState.h>
+ 
 bool isCollisionDetected = false;
-double scanAngle = 15;
-double distanceThreshold = 0.28;
+double scanAngle = 15; 
+double distanceThreshold = 0.28; // distance in meters
+double batteryThreshold = 0.95; // 95% battery threshold for shutdown
+double batteryPercentage = 1.0; // Initial battery level
+
 
 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
+    // 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 
+    // 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()) {
@@ -23,6 +27,12 @@ void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& data)
         }
     }
 }
+ 
+void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& batteryState)
+{
+    batteryPercentage = batteryState->percentage;
+ROS_INFO("The battery is at: %.1f%%",batteryPercentage*100);
+}
 
 int main(int argc, char** argv) {
     ros::init(argc, argv, "autopilot");
@@ -30,25 +40,28 @@ int main(int argc, char** argv) {
 
     ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
     ros::Subscriber laser_sub = nh.subscribe("scan", 1, lidarCallback);
+    ros::Subscriber battery_sub = nh.subscribe("battery_state", 1, batteryCallback);
     ros::Rate loop_rate(10);  // 10 Hz
 
     while (ros::ok()) {
         geometry_msgs::Twist cmd_vel;
 
-       if (isCollisionDetected) {
-            // Spin in place when collision detected
+        if (isCollisionDetected) {
             cmd_vel.linear.x = 0;
             cmd_vel.angular.z = 1;
         } else {
-            // Move forward when no collision
             cmd_vel.linear.x = 0.5;
             cmd_vel.angular.z = 0;
         }
-
         cmd_vel_pub.publish(cmd_vel);
+
+        if (batteryPercentage < batteryThreshold) {
+            ros::shutdown();
+        }
+
         ros::spinOnce();
         loop_rate.sleep();
     }
-
+ 
     return 0;
 }