maxime.lobietti
/
autopilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add documentation and correct values for the thresholds in autopilot.cpp

This commit is contained in:
Justin CHARMETANT 2024-11-09 11:27:08 +01:00
parent 006fda84ab
commit 0e91caf113
1 changed files with 22 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/autopilot.cpp
View File

@ -1,5 +1,15 @@
/**
* \file autopilot.cpp
* \brief Autopilot of a turtlebot3 robot until it mapped most of its environment.
* \author Justin CHARMETANT, Maxime LOBIETTI, Gauthier BOULA DE MAREUIL
* \version 0.1
* \date 07/11/2024
*
* Program that controls a turtlebot3 burger robot using the bringup and slam nodes from ROS noetic.
*
*/
#include <ros/ros.h> #include <ros/ros.h>
#include "std_msgs/String.h"
#include <std_msgs/Float64.h> #include <std_msgs/Float64.h>
#include <sensor_msgs/BatteryState.h> #include <sensor_msgs/BatteryState.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
@ -10,8 +20,8 @@ bool isCollisionDetected = false;
bool isFinished = false; bool isFinished = false;
int angleDetection = 15; int angleDetection = 15;
double distThreshold = 0.3; double distThreshold = 0.3;
float batThreshold = 10; float batThreshold = 10; // We want the voltage of the battery to be above 10V to run the robot.
double entroThreshold = 100000000; double entroThreshold = 0.1; // We want an entropy below 0.1 to stop the program.
// Function to run when receive data from the subscribed Lidar // Function to run when receive data from the subscribed Lidar
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) // void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) //
@ -24,7 +34,7 @@ void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) //
double firstVal = data->ranges[i]; double firstVal = data->ranges[i];
double lastVal = data->ranges[data->ranges.size()-i-1]; double lastVal = data->ranges[data->ranges.size()-i-1];
// Check if one of the data is below the threshold // Check if one of the data is below the threshold distance
if (lastVal <distThreshold && lastVal != 0.0){ if (lastVal <distThreshold && lastVal != 0.0){
isCollisionDetected = true; isCollisionDetected = true;
break; break;
@ -37,13 +47,15 @@ void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) //
} }
void entropyCallback(const std_msgs::Float64& entropy) //?? // Function to run when receive data from the subscribed entropy topic from SLAM
void entropyCallback(const std_msgs::Float64& entropy)
{ {
if (entropy.data > entroThreshold){ if (entropy.data < entroThreshold){
isFinished = true; isFinished = true;
} }
} }
// Function to run when receive data from the subscribed battery_state topic from SLAM
void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& bat) void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& bat)
{ {
if (bat->voltage < batThreshold){ if (bat->voltage < batThreshold){
@ -51,7 +63,8 @@ void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& bat)
} }
} }
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc,argv, "autopilot"); ros::init(argc,argv, "autopilot");
@ -75,7 +88,7 @@ int main(int argc, char** argv)
{ {
geometry_msgs::Twist msg; geometry_msgs::Twist msg;
// If isFinished is true, it means either the map is full or the battery is low, therefore we stop moving
if (!isFinished){ if (!isFinished){
if (isCollisionDetected){ if (isCollisionDetected){
msg.angular.z = 1; msg.angular.z = 1;
@ -84,7 +97,7 @@ int main(int argc, char** argv)
msg.linear.x = 1; msg.linear.x = 1;
} }
} }
cmdVelPub.publish(msg); cmdVelPub.publish(msg);
ros::spinOnce(); ros::spinOnce();
loopRate.sleep(); loopRate.sleep();