baudouin.belpaire
/
Adv_Robotics_Lab2_ROS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

final version

This commit is contained in:
Baudouin BELPAIRE 2022-11-22 17:34:14 +01:00
parent 8368b10444
commit fc873f610c
1 changed files with 30 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/automatic.cpp
View File

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