stateMachine.cpp

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

class FiniteStateMachine {
private:
    ros::NodeHandle nh;

    ros::Subscriber autonomy_sub;
    ros::Subscriber bluetooth_sub;
    ros::Subscriber RC_sub;

    ros::Publisher state_pub;

    geometry_msgs::Twist autonomy_cmd;
    geometry_msgs::Twist bluetooth_cmd;
    geometry_msgs::Twist rc_cmd;

    ros::Time autonomy_last_time;
    ros::Time bluetooth_last_time;
    ros::Time rc_last_time;

    const ros::Duration timeout = ros::Duration(1.0);

    int last_state = 0;
    bool first_publish = true;

public:
    FiniteStateMachine() {
        autonomy_sub = nh.subscribe("cmd_vel", 10, &FiniteStateMachine::autonomyCallback, this);
        bluetooth_sub = nh.subscribe("bluetooth_teleop/cmd_vel", 10, &FiniteStateMachine::bluetoothCallback, this);
        RC_sub = nh.subscribe("joy_teleop/cmd_vel", 10, &FiniteStateMachine::rcCallback, this);

        state_pub = nh.advertise<std_msgs::Int8>("last_robot_state", 10);
    }

    void autonomyCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        autonomy_cmd = *msg;
        autonomy_last_time = ros::Time::now();
    }

    void bluetoothCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        bluetooth_cmd = *msg;
        bluetooth_last_time = ros::Time::now();
    }

    void rcCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        rc_cmd = *msg;
        rc_last_time = ros::Time::now();
    }

    bool isRecent(const ros::Time& last_time) {
        return (ros::Time::now() - last_time) < timeout;
    }

    int computeState(const geometry_msgs::Twist& cmd) {
        if (cmd.linear.x > 0.01) 
            return 1;  // forward
        else if (cmd.linear.x < -0.01) 
            return -1; // backward
        else 
            return 0; // stop
    }

    void run() {
        ros::Rate loop_rate(10);

        while (ros::ok()) {
            int current_state = 0;

            if (isRecent(autonomy_last_time)) {
                current_state = computeState(autonomy_cmd);
            } else if (isRecent(rc_last_time)) {
                current_state = computeState(rc_cmd);
            } else if (isRecent(bluetooth_last_time)) {
                current_state = computeState(bluetooth_cmd);
            }

            if (current_state != last_state || first_publish) {
                std_msgs::Int8 msg;
                msg.data = current_state;
                state_pub.publish(msg);
                last_state = current_state;
                first_publish = false;
            }                


       	ros::spinOnce();
    	loop_rate.sleep();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "finite_state_machine");
    FiniteStateMachine fsm;
    fsm.run();
    return 0;
}