diff --git a/sonar_pkg/CMakeLists.txt b/sonar_pkg/CMakeLists.txt index 40ad7ec..4663593 100644 --- 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 index f6478fb..61079e1 100644 --- a/sonar_pkg/src/sonar.cpp +++ b/sonar_pkg/src/sonar.cpp @@ -1,39 +1,53 @@ #include #include #include -#include +#include +#include #include 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("/cmd_vel", 10); - alert_pub = nh.advertise("reverse_alert", 10); + state_sub = nh.subscribe("last_robot_state", 10, &ReverseChecker::stateCallback, this); + e_stop_pub = nh.advertise("e_stop", 10); object_near = false; - sonars[0] = sonars[1] = sonars[2] = std::numeric_limits::max(); + stop_msg.data = false; + sonars[0] = sonars[1] = sonars[2] = std::numeric_limits::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 new file mode 100644 index 0000000..cd8f769 --- /dev/null +++ b/sonar_pkg/src/stateMachine.cpp @@ -0,0 +1,100 @@ +#include +#include +#include + +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("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; +} +