From 3c7dffc52cd444a92a3f66c17d6d9417c35c3d67 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 9 Jul 2026 18:00:26 -0400 Subject: [PATCH] added in seperate topic --- .../drive_mode_switch_node.hpp | 21 ++++++++ .../src/drive_mode_switch_node.cpp | 53 +++++++++++++++++++ 2 files changed, 74 insertions(+) diff --git a/robot_state_controller/include/robot_state_controller/drive_mode_switch_node.hpp b/robot_state_controller/include/robot_state_controller/drive_mode_switch_node.hpp index f1e71af..b5ae612 100644 --- a/robot_state_controller/include/robot_state_controller/drive_mode_switch_node.hpp +++ b/robot_state_controller/include/robot_state_controller/drive_mode_switch_node.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "ackermann_msgs/msg/ackermann_drive.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -35,6 +36,26 @@ namespace RobotStateController { class DriveModeSwitch : public rclcpp::Node { + + // REDTOO UPDATES +private: + std::mutex state_mutex_; + + // Subscriber for setting the drive mode externally. Level-based and + // idempotent: publishers may re-assert the same mode continuously + // (e.g. the RC switch position at 50 Hz) and nothing happens until + // the requested mode actually differs from the current one. + rclcpp::Subscription::SharedPtr set_drive_mode_subscription_; + + void set_drive_mode_callback(const robot_state_msgs::msg::DriveMode::SharedPtr msg); + + // True at boot and latched again on KILL: AUTONOMOUS commands are + // ignored until a TELEOP command has been seen, so a switch left in + // the auton position can't start the robot autonomous on power-up or + // flip it back to autonomous after an e-stop. + bool auton_locked_out_; + +// REDTOO UPDATES public: explicit DriveModeSwitch(rclcpp::NodeOptions options); diff --git a/robot_state_controller/src/drive_mode_switch_node.cpp b/robot_state_controller/src/drive_mode_switch_node.cpp index b519620..819f8fa 100644 --- a/robot_state_controller/src/drive_mode_switch_node.cpp +++ b/robot_state_controller/src/drive_mode_switch_node.cpp @@ -53,6 +53,16 @@ DriveModeSwitch::DriveModeSwitch(rclcpp::NodeOptions options) : Node("drive_mode last_system_state_ = State::System::ACTIVE; last_drive_mode_state_ = init_value == "teleop" ? State::DriveMode::TELEOP : State::DriveMode::AUTONOMOUS; last_switch_button_pressed_ = false; + // Start locked: if the kart powers on with the RC switch already in the + // auton position, we stay in teleop until teleop has been selected once. + auton_locked_out_ = true; + + // redtoo updates + // Subscription that allows external nodes to command a specific drive mode + set_drive_mode_subscription_ = this->create_subscription( + "/robot/set_drive_mode", 10, + std::bind(&DriveModeSwitch::set_drive_mode_callback, this, std::placeholders::_1)); + // redtoo updates } void DriveModeSwitch::update_params() { this->get_parameter("switch_button", switch_button_); } @@ -65,6 +75,10 @@ void DriveModeSwitch::robot_state_callback(const robot_state_msgs::msg::State::S last_system_state_ = State::System::KILL; // Transition to teleop on kill, so we don't run people over with bots on un-estop. this->last_drive_mode_state_ = State::TELEOP; + // Ignore AUTONOMOUS commands until a TELEOP command re-arms us, + // so a level-based switch left in the auton position can't flip + // us straight back into autonomous after the kill. + auton_locked_out_ = true; drive_mode_msg.drive_mode = robot_state_msgs::msg::DriveMode::TELEOP; drive_mode_publisher_->publish(drive_mode_msg); break; @@ -114,6 +128,45 @@ void DriveModeSwitch::joystick_callback(const sensor_msgs::msg::Joy::SharedPtr m last_switch_button_pressed_ = switch_button_pressed_; } +// REDTOO UPDATES +void DriveModeSwitch::set_drive_mode_callback(const robot_state_msgs::msg::DriveMode::SharedPtr msg) { + State::DriveMode requested; + switch (msg->drive_mode) { + case robot_state_msgs::msg::DriveMode::TELEOP: + requested = State::DriveMode::TELEOP; + break; + case robot_state_msgs::msg::DriveMode::AUTONOMOUS: + requested = State::DriveMode::AUTONOMOUS; + break; + default: + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Ignoring unknown drive mode command: %d", msg->drive_mode); + return; + } + + std::lock_guard lock(state_mutex_); + + // A TELEOP command re-arms autonomous after a kill; until then any + // AUTONOMOUS command (e.g. a switch left in the auton position) is ignored. + if (requested == State::DriveMode::TELEOP) { + auton_locked_out_ = false; + } else if (auton_locked_out_) { + return; + } + + if (requested == last_drive_mode_state_) { + return; + } + + last_drive_mode_state_ = requested; + robot_state_msgs::msg::DriveMode out_msg; + out_msg.drive_mode = msg->drive_mode; + drive_mode_publisher_->publish(out_msg); + RCLCPP_INFO(this->get_logger(), "Drive mode set to %s", + requested == State::DriveMode::TELEOP ? "TELEOP" : "AUTONOMOUS"); +} +// REDTOO UPDATES + void DriveModeSwitch::controller_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) { // Only publish the twist command from the controller // if the robot is in teleop and not killed