94 lines
2.0 KiB
C++
94 lines
2.0 KiB
C++
#include <ros/ros.h>
|
|
#include "std_msgs/String.h"
|
|
#include <std_msgs/Float64.h>
|
|
#include <sensor_msgs/BatteryState.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
#include <sensor_msgs/LaserScan.h>
|
|
|
|
// Parameters
|
|
bool isCollisionDetected = false;
|
|
bool isFinished = false;
|
|
int angleDetection = 15;
|
|
double distThreshold = 0.3;
|
|
float batThreshold = 10;
|
|
double entroThreshold = 100000000;
|
|
|
|
// Function to run when receive data from the subscribed Lidar
|
|
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) //
|
|
{
|
|
isCollisionDetected = false;
|
|
|
|
for (int i=0;i<angleDetection;i++){
|
|
|
|
// Get data in a cone of +-15° in front of the robot
|
|
double firstVal = data->ranges[i];
|
|
double lastVal = data->ranges[data->ranges.size()-i-1];
|
|
|
|
// Check if one of the data is below the threshold
|
|
if (lastVal <distThreshold && lastVal != 0.0){
|
|
isCollisionDetected = true;
|
|
break;
|
|
}
|
|
if (firstVal <distThreshold&& firstVal != 0.0){
|
|
isCollisionDetected = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void entropyCallback(const std_msgs::Float64& entropy) //??
|
|
{
|
|
if (entropy.data > entroThreshold){
|
|
isFinished = true;
|
|
}
|
|
}
|
|
|
|
void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& bat)
|
|
{
|
|
if (bat->voltage < batThreshold){
|
|
isFinished = true;
|
|
}
|
|
}
|
|
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
ros::init(argc,argv, "autopilot");
|
|
|
|
// Publish to the cmd_vel topic
|
|
ros::NodeHandle nh;
|
|
ros::Publisher cmdVelPub = nh.advertise<geometry_msgs::Twist>("cmd_vel",1);
|
|
|
|
// Subscribe to the scan topic
|
|
ros::Subscriber scanSub = nh.subscribe("scan",1,scanCallback);
|
|
|
|
// Subscribe to the entropy topic
|
|
ros::Subscriber entropySub = nh.subscribe("turtlebot3_slam_gmapping/entropy",1,entropyCallback);
|
|
|
|
// Subscribe to the battery state topic
|
|
ros::Subscriber batterySub = nh.subscribe("battery_state",1,batteryCallback);
|
|
|
|
ros::Rate loopRate(10);
|
|
|
|
while(ros::ok())
|
|
{
|
|
geometry_msgs::Twist msg;
|
|
|
|
|
|
if (!isFinished){
|
|
if (isCollisionDetected){
|
|
msg.angular.z = 1;
|
|
}
|
|
else{
|
|
msg.linear.x = 1;
|
|
}
|
|
}
|
|
|
|
cmdVelPub.publish(msg);
|
|
ros::spinOnce();
|
|
loopRate.sleep();
|
|
}
|
|
return 0;
|
|
}
|