sonar.cpp

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Int8.h>
#include <limits>

class ReverseChecker {
private:
    ros::NodeHandle nh;

    ros::Subscriber sonar_sub;
    ros::Subscriber state_sub;

    ros::Publisher alert_pub;
    ros::Publisher e_stop_pub;

    ros::Time last_e_stop_time;
    std_msgs::Bool stop_msg;

    const int LIMIT_DISTANCE = 200;

    bool object_near;
    int sonars[3];
    int state_input;

public:
    ReverseChecker() {
        sonar_sub = nh.subscribe("sonar_array", 10, &ReverseChecker::sonarCallback, this);
        state_sub = nh.subscribe("last_robot_state", 10, &ReverseChecker::stateCallback, this);
	    e_stop_pub = nh.advertise<std_msgs::Bool>("e_stop", 10);

        object_near = false;
	    stop_msg.data = false;
        sonars[0] = sonars[1] = sonars[2] = std::numeric_limits<int>::max();
        last_e_stop_time = ros::Time(0);
    }

    void sonarCallback(const std_msgs::Int32MultiArray::ConstPtr& msg) {
        if (msg->data.size() < 3) {
            ROS_WARN("Insufficient data in sonar_array");
            return;
        }

        sonars[0] = msg->data[0];
        sonars[1] = msg->data[1];
        sonars[2] = msg->data[2];

        object_near = false;

        for (int i = 0; i < 3; i++) {
            if (sonars[i] > 0 && sonars[i] < LIMIT_DISTANCE) {
                object_near = true;
                break;
            }
        }
    }

    void stateCallback(const std_msgs::Int8::ConstPtr& msg) {
        state_input = msg->data;
    }

    void run() {
        ros::Rate loop_rate(100);
        bool stop_active = false;

        while (ros::ok()) {
        ros::Time now = ros::Time::now();
        bool reversing = (state_input == -1);

        if (!stop_active && object_near && reversing) {
            stop_active = true;
            last_e_stop_time = now;
            ROS_WARN("Obstacle detected while reversing. Emergency stop activated!");
        }

        if (stop_active) {
            ros::Duration stop_duration = now - last_e_stop_time;

            if (stop_duration < ros::Duration(1.0)) {
                stop_msg.data = true;
            } else {
                if (!reversing || !object_near) {
                    stop_active = false;
                    stop_msg.data = false;
                    ROS_INFO("Emergency stop released.");
                } else {
                    stop_msg.data = true;
                }
            }
        } else {
            stop_msg.data = false;
        }

	    e_stop_pub.publish(stop_msg);
       	ros::spinOnce();
    	loop_rate.sleep();

        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "reverse_checker");
    ReverseChecker checker;
    checker.run();
    return 0;
}