Operation mode manager implementation #656

eirisak · 2026-01-08T13:35:03Z

Added service node:
operation_mode_manager.cpp

Changed joystick interface to make use of the manager

Added Operation mode service request type, aswell as message type to use for the operation mode topic

Changed various dp controls to accomodate these changes.

Added operation mode manager to orca.launch.py

Also added operation mode to stonefish_sim in this pull request:
vortexntnu/vortex-stonefish-sim#18

…ingly

…nager

Andeshog · 2026-01-08T14:51:26Z

mission/keyboard_joy/keyboard_joy/keyboard_joy_node.py

These files should probably not be a part of this PR, as they are part of a separate PR?

Andeshog · 2026-01-08T14:58:04Z

mission/operation_mode_manager/operation_mode_manager/operation_mode_manager.cpp

+    void declare_parameters()
+    {
+      this->declare_parameter<std::string>("topics.wrench_input", "");
+      this->declare_parameter<std::string>("topics.killswitch", "");
+      this->declare_parameter<std::string>("topics.operation_mode", "");
+
+      wrench_input_topic_ =
+        this->get_parameter("topics.wrench_input").as_string();
+      killswitch_topic_ =
+        this->get_parameter("topics.killswitch").as_string();
+      operation_mode_topic_ =
+        this->get_parameter("topics.operation_mode").as_string();
+    }
+
+    void setup_publishers()
+    {
+      wrench_pub_ =
+        this->create_publisher<geometry_msgs::msg::WrenchStamped>(
+          wrench_input_topic_, rclcpp::SensorDataQoS());
+
+      killswitch_pub_ =
+        this->create_publisher<std_msgs::msg::Bool>(
+          killswitch_topic_, rclcpp::QoS(2).reliable());
+
+      mode_pub_ =
+        this->create_publisher<vortex_msgs::msg::OperationMode>(
+          operation_mode_topic_, rclcpp::QoS(2).reliable());


Instead of keeping the topics as member variables I would just get them in the setup_publishers method

void declare_parameters() { this->declare_parameter<std::string>("topics.wrench_input"); this->declare_parameter<std::string>("topics.killswitch"); this->declare_parameter<std::string>("topics.operation_mode"); } void setup_publishers() { const auto wrench_input_topic = this->get_parameter("topics.wrench_input").as_string(); const auto killswitch_topic = this->get_parameter("topics.killswitch").as_string(); const auto operation_mode_topic = this->get_parameter("topics.operation_mode").as_string(); wrench_pub_ = this->create_publisher<geometry_msgs::msg::WrenchStamped>( wrench_input_topic, rclcpp::SensorDataQoS()); killswitch_pub_ = this->create_publisher<std_msgs::msg::Bool>( killswitch_topic, rclcpp::QoS(2).reliable()); mode_pub_ = this->create_publisher<vortex_msgs::msg::OperationMode>( operation_mode_topic, rclcpp::QoS(2).reliable()); ...

Also, there are some QoS definitions in vortex utils ros 👍

jorgenfj

First round of review.

Consider splitting into header and source file, and move declarations into the header file.

Also use spdlog for logging instead of rclcpp. Spdlog gives more readable timestamps. You can look at the dp_adapt_backs_controller package for how to import and use spdlog.

jorgenfj · 2026-01-08T15:19:23Z

mission/operation_mode_manager/operation_mode_manager/operation_mode_manager.cpp

+
+    void declare_parameters()
+    {
+      this->declare_parameter<std::string>("topics.wrench_input", "");


Should avoid using default values for ros parameters

jorgenfj · 2026-01-08T15:20:02Z

mission/operation_mode_manager/operation_mode_manager/operation_mode_manager.cpp

+    std::string wrench_input_topic_;
+    std::string killswitch_topic_;
+    std::string operation_mode_topic_;


These should not be member varaibles since they are only used once

jorgenfj · 2026-01-08T15:20:49Z

mission/operation_mode_manager/operation_mode_manager/operation_mode_manager.cpp

+      killswitch_(true),
+      mode_(vortex_msgs::srv::OperationModeSRV::Request::MANUAL)


Can these values be set from ros parameters and retrieved from the config file in this package?

jorgenfj · 2026-01-08T15:22:28Z

mission/operation_mode_manager/operation_mode_manager/operation_mode_manager.cpp

+      // Initial killswitch = true
+      std_msgs::msg::Bool msg;
+      msg.data = true;
+      killswitch_pub_->publish(msg);
+
+      // Initial mode = MANUAL
+      vortex_msgs::msg::OperationMode mode_msg;
+      mode_msg.mode = vortex_msgs::srv::OperationModeSRV::Request::MANUAL;
+      mode_pub_->publish(mode_msg);


This should read from the member variables

jorgenfj · 2026-01-08T15:26:22Z

mission/operation_mode_manager/operation_mode_manager/operation_mode_manager.cpp

+    // Publish killswitch status
+    std_msgs::msg::Bool killswitch_msg;
+    killswitch_msg.data = killswitch_;
+    killswitch_pub_->publish(killswitch_msg); 
+
+    // Publish operation mode
+    vortex_msgs::msg::OperationMode mode_msg;
+    mode_msg.mode = mode_;
+    mode_pub_->publish(mode_msg); 


make these helper functions

Andeshog · 2026-01-08T17:04:53Z

auv_setup/launch/orca.launch.py

-        output="screen",
+        output="screen",killswitch


What is this?

Andeshog · 2026-01-09T18:26:30Z

...p_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp

-    std::string software_mode_{};
+    uint8_t software_mode_{};


To make mode more strongly typed I would consider making an enum class for it, and make a convertion which can be used in the callback. Something like

enum class Mode : uint8_t {manual, autonomous, reference}; // Mode type instead of raw uint8 Mode software_mode_{}; // Free function (not part of the node class) Mode convert_from_ros(const vortex_msgs::msg::OperationMode& mode_msg) { switch (mode_msg.mode) { case vortex_msgs::msg::OperationMode::MANUAL: return Mode::manual; case vortex_msgs::msg::OperationMode::AUTONOMOUS: return Mode::autonomous; case vortex_msgs::msg::OperationMode::REFERENCE: return Mode::reference; } assert(false && "Unknown operation mode"); } // Callback void DPAdaptBacksControllerNode::software_mode_callback( const vortex_msgs::msg::OperationMode::SharedPtr msg) { software_mode_ = convert_from_ros(*msg); // Either A) make a to_string helper function or B) use magic_enum to convert enum to string spdlog::info("Software mode: {}", magic_enum::enum_name(software_mode_)); if (software_mode_ == Mode::autonomous || software_mode_ == Mode::reference) { pose_d_ = pose_; } }

Note the part about printing enum name. This feedback goes for other places in the code as well where mode is just stored as a raw integer

Andeshog · 2026-01-09T18:31:08Z

mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py

+        self.operation_mode_client = self.create_client(OperationModeSRV, 'set_operation_mode')
+
+        while not self.operation_mode_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Operation Mode service not available, waiting...')
+
+        self.toggle_killswitch_client = self.create_client(ToggleKillswitch, 'toggle_killswitch')
+
+        while not self.toggle_killswitch_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Toggle Killswitch service not available, waiting...')


make a set_services (to keep the naming consistent). Alternatively find a more fitting prefix than "set" for both that and self.set_publishers_and_subscribers()

Andeshog · 2026-01-09T18:37:48Z

mission/joystick_interface_auv/joystick_interface_auv/joystick_utils.py

+modes = {OperationMode.AUTONOMOUS:"Autonomous", OperationMode.MANUAL:"Manual", OperationMode.REFERENCE:"Reference"}



Better to rely on Enum here than dictionary + string

Andeshog · 2026-01-09T18:39:03Z

mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py

+        self._killswitch = 0
+        self._mode = 0


use bool for killswitch, and would advise use the enum (see comment in utils, and the comment in dp controller) for mode

Andeshog · 2026-01-09T18:43:05Z

mission/operation_mode_manager/src/operation_mode_manager.cpp

+      wrench_input_topic, rclcpp::SensorDataQoS());
+


sensor_data_profile in vortex_utils_ros?

Andeshog · 2026-01-09T18:47:40Z

mission/operation_mode_manager/src/operation_mode_manager.cpp

+    [this](
+      const std::shared_ptr<vortex_msgs::srv::ToggleKillswitch::Request> request,
+      std::shared_ptr<vortex_msgs::srv::ToggleKillswitch::Response> response)
+    {
+      (void)request;


prefer

const std::shared_ptr<vortex_msgs::srv::ToggleKillswitch::Request> /*request*/, std::shared_ptr<vortex_msgs::srv::ToggleKillswitch::Response> response

when not using a parameter, instead of (void).... Alternatively follow F.9

kluge7 and others added 5 commits November 4, 2025 02:35

feat(joy): add keyboard_joy node for keyboard-based teleoperation

fcfa385

chore(keyboard_joy_node): removed comments

eec9cb3

Made Operation Mode manager. Added to launch files. Changed dp accord…

9386455

…ingly

Merge remote-tracking branch 'origin' into 638-task-operation-mode-ma…

f6af037

…nager

Fixed launch from orca_sim + orca for operation mode manager + polish

0421d64

eirisak requested review from Andeshog, Q3rkses, jorgenfj and kluge7 January 8, 2026 13:35

eirisak self-assigned this Jan 8, 2026

eirisak linked an issue Jan 8, 2026 that may be closed by this pull request

[TASK] Operation Mode Manager #638

Open

3 tasks

eirisak mentioned this pull request Jan 8, 2026

Added operation mode manager to launch vortexntnu/vortex-stonefish-sim#18

Draft

eirisak added 2 commits January 8, 2026 15:32

Added dependency to package.xml for operation mode manager

df3a299

deleted operation mode python files

ea4fcbf

Andeshog reviewed Jan 8, 2026

View reviewed changes

deleted keyboard joy

4f8cfe2

Andeshog reviewed Jan 8, 2026

View reviewed changes

jorgenfj requested changes Jan 8, 2026

View reviewed changes

Andeshog reviewed Jan 8, 2026

View reviewed changes

auv_setup/launch/orca.launch.py Outdated

Comment on lines 63 to 73

output="screen",

output="screen",killswitch

Copy link

Contributor

Andeshog Jan 8, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What is this?

Polish according to pull request comments

8c7c93b

Andeshog reviewed Jan 9, 2026

View reviewed changes

		killswitch_(true),
		mode_(vortex_msgs::srv::OperationModeSRV::Request::MANUAL)

		modes = {OperationMode.AUTONOMOUS:"Autonomous", OperationMode.MANUAL:"Manual", OperationMode.REFERENCE:"Reference"}

Operation mode manager implementation #656

Are you sure you want to change the base?

Operation mode manager implementation #656

Uh oh!

Conversation

eirisak commented Jan 8, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

jorgenfj left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

eirisak commented Jan 8, 2026 •

edited

Loading