added in seperate topic for contious updates

robot_state_controller/include/robot_state_controller/drive_mode_switch_node.hpp

@@ -23,6 +23,7 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <mutex>
 
 #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<robot_state_msgs::msg::DriveMode>::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);
 

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_state_msgs::msg::DriveMode>(
+        "/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<std::mutex> 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