verlab · Ronaldo-gsa · May 13, 2025
diff --git a/sonar_pkg/CMakeLists.txt b/sonar_pkg/CMakeLists.txt
@@ -216,9 +216,11 @@ find_package(catkin REQUIRED COMPONENTS
 add_executable(sonar src/sonar.cpp)
 target_link_libraries(sonar ${catkin_LIBRARIES})
 
-# Só cria o executável se CATKIN_ENABLE_TESTING estiver ativo
+add_executable(stateMachine src/stateMachine.cpp)
+target_link_libraries(stateMachine ${catkin_LIBRARIES})
+
 #if (CATKIN_ENABLE_TESTING) #Tirar no código definitivo
-  find_package(rostest REQUIRED)  # Garante que o rostest está disponível
+  find_package(rostest REQUIRED)
 
   catkin_add_gtest(test_reverse_checker test/test_reverse_checker.cpp)
   target_link_libraries(test_reverse_checker ${catkin_LIBRARIES})

diff --git a/sonar_pkg/src/sonar.cpp b/sonar_pkg/src/sonar.cpp
@@ -1,39 +1,53 @@
 #include <ros/ros.h>
 #include <std_msgs/Bool.h>
 #include <geometry_msgs/Twist.h>
-#include <geometry_msgs/Vector3.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 velocity_sub;
+    ros::Subscriber state_sub;
+
     ros::Publisher alert_pub;
-    ros::Publisher velocity_pub;
-    bool object_near;
-    geometry_msgs::Twist current_velocity;
-    double sonars[3];
+    ros::Publisher e_stop_pub;
 
-    const int LIMIT_DISTANCE = 500;
+    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);
-        velocity_sub = nh.subscribe("/cmd_vel", 10, &ReverseChecker::velocityCallback, this);
-        velocity_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
-        alert_pub = nh.advertise<std_msgs::Bool>("reverse_alert", 10);
+        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;
-        sonars[0] = sonars[1] = sonars[2] = std::numeric_limits<double>::max();
+	    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 geometry_msgs::Vector3::ConstPtr& msg) {
-        sonars[0] = msg->x;
-        sonars[1] = msg->y;
-        sonars[2] = msg->z;
+    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;
@@ -42,25 +56,46 @@ class ReverseChecker {
         }
     }
 
-    void velocityCallback(const geometry_msgs::Twist::ConstPtr& msg) {
-        current_velocity = *msg;
+    void stateCallback(const std_msgs::Int8::ConstPtr& msg) {
+        state_input = msg->data;
     }
 
     void run() {
-        ros::Rate loop_rate(10);
+        ros::Rate loop_rate(100);
+        bool stop_active = false;
+
         while (ros::ok()) {
-			if (object_near){
-				ROS_WARN("Object near detect...");
-                ROS_WARN_STREAM("Current velocity X: " << current_velocity.linear.x);
-				if (current_velocity.linear.x < 0){
-					ROS_WARN("Moving in reverse near an obstacle! Stopping...");
-					geometry_msgs::Twist stop_msg;
-                	velocity_pub.publish(stop_msg);
-				}
-			}
-
-            ros::spinOnce();
-            loop_rate.sleep();
+        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();
+
         }
     }
 };

diff --git a/sonar_pkg/src/stateMachine.cpp b/sonar_pkg/src/stateMachine.cpp
@@ -0,0 +1,100 @@
+#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;
+}
+