86 lines
2.3 KiB
C++
Executable File
86 lines
2.3 KiB
C++
Executable File
#include "ros/ros.h"
|
|
#include "geometry_msgs/Twist.h"
|
|
#include "sensor_msgs/LaserScan.h"
|
|
#include <cstdlib> // For rand() and RAND_MAX
|
|
#include <ctime> // For time()
|
|
#include <vector>
|
|
|
|
// Global variables
|
|
std::vector<float> distance_data;
|
|
ros::Publisher pub;
|
|
|
|
// Function declarations
|
|
void stop_robot();
|
|
bool detect_obstacle(float range_val);
|
|
|
|
void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
|
|
distance_data = msg->ranges;
|
|
}
|
|
|
|
bool detect_obstacle(float range_val) {
|
|
for (float i : distance_data) {
|
|
if (i <= range_val) {
|
|
return true; // Obstacle detected
|
|
}
|
|
}
|
|
return false; // No obstacle
|
|
}
|
|
|
|
void stop_robot() {
|
|
geometry_msgs::Twist vel_msg;
|
|
vel_msg.linear.x = 0.0;
|
|
vel_msg.angular.z = 0.0;
|
|
pub.publish(vel_msg);
|
|
}
|
|
|
|
void move_forward() {
|
|
if (detect_obstacle(0.5)) {
|
|
stop_robot(); // Obstacle detected, stop the robot
|
|
} else {
|
|
geometry_msgs::Twist vel_msg;
|
|
vel_msg.linear.x = 0.5;
|
|
pub.publish(vel_msg); // No obstacle, move forward
|
|
}
|
|
}
|
|
|
|
int main(int argc, char **argv) {
|
|
// Initialize the ROS system and become a node.
|
|
ros::init(argc, argv, "autonomous_exploration_node");
|
|
ros::NodeHandle nh;
|
|
|
|
// Create a publisher object.
|
|
pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
|
|
|
|
// Create a subscriber for the laser scanner data.
|
|
ros::Subscriber sub = nh.subscribe("/scan", 1000, laser_callback);
|
|
|
|
// Seed the random number generator.
|
|
srand(time(0));
|
|
|
|
// Capture the start time to calculate elapsed time later.
|
|
ros::Time start_time = ros::Time::now();
|
|
|
|
// Define the operation duration.
|
|
ros::Duration duration(10.0); // 10 seconds
|
|
|
|
ros::Rate rate(10); // 10 Hz
|
|
|
|
// Main loop.
|
|
while (ros::ok()) {
|
|
// Decision-making logic to move or rotate.
|
|
move_forward();
|
|
|
|
// Check if 10 seconds have passed.
|
|
if (ros::Time::now() - start_time > duration) {
|
|
// If 10 seconds have passed, stop the robot and exit the loop.
|
|
stop_robot();
|
|
break;
|
|
}
|
|
|
|
ros::spinOnce(); // Call this function often to allow ROS to process incoming messages.
|
|
rate.sleep(); // Sleep for the rest of the cycle, to enforce the loop rate.
|
|
}
|
|
return 0; // Exit normally.
|
|
}
|
|
|