Merge pull request #656 from vortexntnu/638-task-operation-mode-manager

Operation mode manager implementation

:3

Author: eirisak

auv_setup/config/robots/orca.yaml

@@ -136,6 +136,12 @@
       reference_filter: "reference_filter"
       los: "los_guidance"
 
+    services:
+      set_operation_mode: "set_operation_mode"
+      set_killswitch: "set_killswitch"
+      toggle_killswitch: "toggle_killswitch"
+      get_operation_mode: "get_operation_mode"
+
     fsm:
       docking:
         docking_station_offset: -1.0

auv_setup/launch/topside.launch.py

@@ -44,4 +44,10 @@ def generate_launch_description() -> LaunchDescription:
         )
     )
 
-    return LaunchDescription([set_env_var, joy_node, joystick_interface_launch])
+    return LaunchDescription(
+        [
+            set_env_var,
+            joy_node,
+            joystick_interface_launch,
+        ]
+    )

control/dp_adapt_backs_controller/CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(vortex_msgs REQUIRED)
 find_package(fmt REQUIRED)
 find_package(spdlog REQUIRED)
 
+
 include_directories(include)
 
 set(LIB_NAME "${PROJECT_NAME}_component")

control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp

@@ -14,7 +14,9 @@
 #include <std_msgs/msg/string.hpp>
 #include <string>
 #include <vortex/utils/types.hpp>
+#include <vortex_msgs/msg/operation_mode.hpp>
 #include <vortex_msgs/msg/reference_filter.hpp>
+#include <vortex_msgs/srv/get_operation_mode.hpp>
 #include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp"
 #include "dp_adapt_backs_controller/typedefs.hpp"
 #include "typedefs.hpp"
@@ -28,13 +30,18 @@ class DPAdaptBacksControllerNode : public rclcpp::Node {
         const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
 
    private:
+    // @brief Client for the GetOperationMode service
+    rclcpp::Client<vortex_msgs::srv::GetOperationMode>::SharedPtr
+        get_operation_mode_client_;
+
     // @brief Callback function for the killswitch topic
     // @param msg: Bool message containing the killswitch status
     void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg);
 
     // @brief Callback function for the software mode topic
     // @param msg: String message containing the software mode
-    void software_mode_callback(const std_msgs::msg::String::SharedPtr msg);
+    void operation_mode_callback(
+        const vortex_msgs::msg::OperationMode::SharedPtr msg);
 
     // @brief Callback function for the pose topic
     // @param msg: PoseWithCovarianceStamped message containing the AUV pose
@@ -55,6 +62,10 @@ class DPAdaptBacksControllerNode : public rclcpp::Node {
     // @brief Set the subscriber and publisher for the node
     void set_subscribers_and_publisher();
 
+    // @brief Initialize the operation mode by calling the GetOperationMode
+    // service
+    void initialize_operation_mode();
+
     // @brief Callback function for the guidance topic
     // @param msg: ReferenceFilter message containing the desired vehicle pose
     // and velocity
@@ -63,7 +74,8 @@ class DPAdaptBacksControllerNode : public rclcpp::Node {
 
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr killswitch_sub_{};
 
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr software_mode_sub_{};
+    rclcpp::Subscription<vortex_msgs::msg::OperationMode>::SharedPtr
+        operation_mode_sub_{};
 
     rclcpp::Subscription<
         geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_{};
@@ -88,9 +100,10 @@ class DPAdaptBacksControllerNode : public rclcpp::Node {
 
     std::unique_ptr<DPAdaptBacksController> dp_adapt_backs_controller_{};
 
-    bool killswitch_on_{false};
+    bool killswitch_on_{true};
 
-    std::string software_mode_{};
+    vortex::utils::types::Mode operation_mode_{
+        vortex::utils::types::Mode::manual};
 };
 
 }  // namespace vortex::control

control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp

@@ -4,6 +4,7 @@
 #include <string>
 #include <vortex/utils/math.hpp>
 #include <vortex/utils/ros/qos_profiles.hpp>
+#include <vortex/utils/ros/ros_conversions.hpp>
 #include <vortex/utils/types.hpp>
 #include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp"
 #include "dp_adapt_backs_controller/typedefs.hpp"
@@ -25,6 +26,7 @@ DPAdaptBacksControllerNode::DPAdaptBacksControllerNode(
     time_step_ = std::chrono::milliseconds(10);
 
     set_subscribers_and_publisher();
+    initialize_operation_mode();
 
     tau_pub_timer_ = this->create_wall_timer(
         time_step_, std::bind(&DPAdaptBacksControllerNode::publish_tau, this));
@@ -74,10 +76,11 @@ void DPAdaptBacksControllerNode::set_subscribers_and_publisher() {
     this->declare_parameter<std::string>("topics.operation_mode");
     std::string software_operation_mode_topic =
         this->get_parameter("topics.operation_mode").as_string();
-    software_mode_sub_ = this->create_subscription<std_msgs::msg::String>(
-        software_operation_mode_topic, qos_reliable,
-        std::bind(&DPAdaptBacksControllerNode::software_mode_callback, this,
-                  std::placeholders::_1));
+    operation_mode_sub_ =
+        this->create_subscription<vortex_msgs::msg::OperationMode>(
+            software_operation_mode_topic, qos_reliable,
+            std::bind(&DPAdaptBacksControllerNode::operation_mode_callback,
+                      this, std::placeholders::_1));
 
     this->declare_parameter<std::string>("topics.wrench_input");
     std::string control_topic =
@@ -86,6 +89,44 @@ void DPAdaptBacksControllerNode::set_subscribers_and_publisher() {
         control_topic, qos_sensor_data);
 }
 
+void DPAdaptBacksControllerNode::initialize_operation_mode() {
+    this->declare_parameter<std::string>("services.get_operation_mode");
+    std::string get_operation_mode_service =
+        this->get_parameter("services.get_operation_mode").as_string();
+
+    get_operation_mode_client_ =
+        this->create_client<vortex_msgs::srv::GetOperationMode>(
+            get_operation_mode_service);
+
+    while (!get_operation_mode_client_->wait_for_service(
+        std::chrono::seconds(1))) {
+        spdlog::warn("Waiting for GetOperationMode service to be available...");
+    }
+
+    auto request =
+        std::make_shared<vortex_msgs::srv::GetOperationMode::Request>();
+    get_operation_mode_client_->async_send_request(
+        request,
+        [this](rclcpp::Client<vortex_msgs::srv::GetOperationMode>::SharedFuture
+                   future) {
+            try {
+                auto response = future.get();
+                operation_mode_ =
+                    vortex::utils::ros_conversions::convert_from_ros(
+                        response->current_operation_mode);
+                killswitch_on_ = response->killswitch_status;
+                spdlog::info(
+                    "Initial operation mode: {} | Killswitch status: {}",
+                    vortex::utils::types::mode_to_string(operation_mode_),
+                    killswitch_on_ ? "on" : "off");
+            } catch (const std::exception& e) {
+                spdlog::error("Failed to get initial operation mode: {}",
+                              e.what());
+                killswitch_on_ = true;
+            }
+        });
+}
+
 void DPAdaptBacksControllerNode::killswitch_callback(
     const std_msgs::msg::Bool::SharedPtr msg) {
     killswitch_on_ = msg->data;
@@ -94,12 +135,14 @@ void DPAdaptBacksControllerNode::killswitch_callback(
     spdlog::info("Killswitch: {}", killswitch_on_ ? "on" : "off");
 }
 
-void DPAdaptBacksControllerNode::software_mode_callback(
-    const std_msgs::msg::String::SharedPtr msg) {
-    software_mode_ = msg->data;
-    spdlog::info("Software mode: {}", software_mode_);
+void DPAdaptBacksControllerNode::operation_mode_callback(
+    const vortex_msgs::msg::OperationMode::SharedPtr msg) {
+    operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg);
+    spdlog::info("Operation mode: {}",
+                 vortex::utils::types::mode_to_string(operation_mode_));
 
-    if (software_mode_ == "autonomous mode") {
+    if (operation_mode_ == vortex::utils::types::Mode::autonomous ||
+        operation_mode_ == vortex::utils::types::Mode::reference) {
         pose_d_ = pose_;
     }
 }
@@ -180,7 +223,8 @@ void DPAdaptBacksControllerNode::set_adap_params() {
 }
 
 void DPAdaptBacksControllerNode::publish_tau() {
-    if (killswitch_on_ || software_mode_ != "autonomous mode") {
+    if (killswitch_on_ ||
+        operation_mode_ == vortex::utils::types::Mode::manual) {
         return;
     }
 

control/pid_controller_dp/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8)
 project(pid_controller_dp)
 
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD 20)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -16,6 +16,8 @@ find_package(geometry_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
 
 include_directories(include)
 
@@ -34,6 +36,8 @@ ament_target_dependencies(pid_controller_node
   Eigen3
   tf2
   vortex_msgs
+  vortex_utils
+  vortex_utils_ros
 )
 
 install(TARGETS

control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp

@@ -13,7 +13,10 @@
 #include <std_msgs/msg/string.hpp>
 #include <string>
 #include <variant>
+#include <vortex/utils/types.hpp>
+#include <vortex_msgs/msg/operation_mode.hpp>
 #include <vortex_msgs/msg/reference_filter.hpp>
+#include <vortex_msgs/srv/get_operation_mode.hpp>
 #include "pid_controller_dp/pid_controller.hpp"
 #include "pid_controller_dp/typedefs.hpp"
 
@@ -29,7 +32,8 @@ class PIDControllerNode : public rclcpp::Node {
 
     // @brief Callback function for the software mode topic
     // @param msg: String message containing the software mode
-    void software_mode_callback(const std_msgs::msg::String::SharedPtr msg);
+    void operation_mode_callback(
+        const vortex_msgs::msg::OperationMode::SharedPtr msg);
 
     // @brief Callback function for the pose topic
     // @param msg: PoseWithCovarianceStamped message containing the AUV pose
@@ -50,17 +54,25 @@ class PIDControllerNode : public rclcpp::Node {
     // @brief Set the subscriber and publisher for the node
     void set_subscribers_and_publisher();
 
+    // @brief Initialize the operation mode by calling the GetOperationMode
+    // service
+    void initialize_operation_mode();
+
     // @brief Callback function for the guidance topic
     // @param msg: ReferenceFilter message containing the desired vehicle pose
     // and velocity
     void guidance_callback(
         const vortex_msgs::msg::ReferenceFilter::SharedPtr msg);
 
+    rclcpp::Client<vortex_msgs::srv::GetOperationMode>::SharedPtr
+        get_operation_mode_client_;
+
     PIDController pid_controller_;
 
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr killswitch_sub_;
 
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr software_mode_sub_;
+    rclcpp::Subscription<vortex_msgs::msg::OperationMode>::SharedPtr
+        operation_mode_sub_;
 
     rclcpp::Subscription<
         geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
@@ -91,9 +103,10 @@ class PIDControllerNode : public rclcpp::Node {
 
     types::Eta eta_dot_d_;
 
-    bool killswitch_on_;
+    bool killswitch_on_{true};
 
-    std::string software_mode_;
+    vortex::utils::types::Mode operation_mode_{
+        vortex::utils::types::Mode::manual};
 };
 
 #endif  // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_

control/pid_controller_dp/package.xml

@@ -15,6 +15,8 @@
   <depend>eigen</depend>
   <depend>tf2</depend>
   <depend>vortex_msgs</depend>
+  <depend>vortex_utils</depend>
+  <depend>vortex_utils_ros</depend>
 
   <export>
     <build_type>ament_cmake</build_type>