diff --git a/src/automatic.cpp b/src/automatic.cpp index f433d83..2b7b863 100644 --- a/src/automatic.cpp +++ b/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("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();