final version

src/automatic.cpp

@@ -2,40 +2,59 @@
 #include "std_msgs/String.h"
 #include "geometry_msgs/Twist.h"
 #include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/BatteryState.h"
+#include "time.h"
 
 bool isObstacle = false;
+bool isBattery = false;
 
 void scanCallback(const sensor_msgs::LaserScan msgScan){
 	//ROS_INFO("I heard [%f]:", msgScan.ranges);
 	isObstacle=false;
 	for(int i=165;i<196;i++){
-		if(msgScan.ranges[i]<0.2)
+                if(msgScan.ranges[i]<0.1)
 			{
 			isObstacle=true;
 			}
 	}
 }
 
+void batteryCallback(const sensor_msgs::BatteryState msgBattery){
+    if(msgBattery.voltage>11){
+        isBattery=true;
+    }
+    else{
+        isBattery=false;
+    }
+}
+
+
+
 int main(int argc,char** argv){
 	ros::init(argc,argv,"automatic");
 	ros::NodeHandle nh;
 	ros::Publisher cmdPublisher = nh.advertise<geometry_msgs::Twist>("cmd_vel",1);
 	ros::Subscriber scanListener = nh.subscribe("scan",10,scanCallback);
-	ros::Rate loopRate(10);
-	
+        ros::Rate loopRate(30);
+        int runtime;
+        int starttime=time(NULL);
 	while(ros::ok())
 	{
+                runtime = time(NULL);
+                int difftime=runtime-starttime;
 		geometry_msgs::Twist message;
 		message.linear.x=0;
 		message.angular.z=0;
-		if(isObstacle==false)
-		{
-			message.linear.x=-1;
-		}
-		if(isObstacle==true)
-		{
-			message.angular.z=1;
-		}
+                if (difftime<60 || isBattery==true){
+                    if(isObstacle==false)
+                    {
+                            message.linear.x=-1;
+                    }
+                    if(isObstacle==true)
+                    {
+                            message.angular.z=1;
+                    }
+                }
 		cmdPublisher.publish(message);
 		ros::spinOnce();
 		loopRate.sleep();