bombav3

src/autopilot.cpp

@@ -1,14 +1,18 @@
 #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 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  
     int cone_size = (data->ranges.size() * scanAngle) / 360;  // x degrees on each side
@@ -24,28 +28,37 @@ 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");
     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::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
             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();
     }