last update before branch protected

interfacing_nav_cs/interfacing_nav_cs/NavCSInterface.py

@@ -32,8 +32,8 @@ def __init__(self):
         self.mode = 'Off'
 
         # Change Mode
-        self.cs_request = self.create_service(ChangeModeSystem, '/ROVER/NAV_mode', self.execute_service)
-        #self.mode_publisher = self.create_publisher(String, '/NAV/mode', 1)
+        self.cs_request = self.create_service(ChangeModeSystem, '/ROVER/change_NAV_mode', self.execute_service)
+        self.mode_publisher = self.create_publisher(String, '/ROVER/NAV_mode', 1)
 
         # Nav 2
         self.path_nav2_launch_file = '/dev_ws/src/path_planning/launch/nav2_real.launch.py'
@@ -48,16 +48,6 @@ def __init__(self):
                                                       '/NAV_motor_cmds/change_state')
         self.motor_check_service = self.create_client(GetState, 
                                                       '/NAV_motor_cmds/get_state')
-
-        # gamepad
-        self.recieve_gamepad = self.create_subscription(Joy, '/CS/NAV_gamepad', self.process_gamepad, 10)
-
-    # ----------------------------------------------------------
-    # GAMEPAD
-
-    def process_gamepad(self, msg):
-        if self.mode != 'Manual': return
-        print(msg)
 
     # ----------------------------------------------------------
     # SERVICE

wheels_control/src/Cmd_vel_manager_node.cpp

@@ -54,37 +54,18 @@ class CmdvelManager : public rclcpp::Node
       sub_cmd_vel_manual = this->create_subscription<geometry_msgs::msg::Twist>(
         "/NAV/cmd_vel_manual", 10, std::bind(&CmdvelManager::callback_cmd_vel_manual, this, std::placeholders::_1));
 
-
       sub_cmd_vel_auto = this->create_subscription<geometry_msgs::msg::Twist>(
         "/cmd_vel", 10, std::bind(&CmdvelManager::callback_cmd_vel_auto, this, std::placeholders::_1));
 
-
-      sub_cmd_shutdown = this->create_subscription<std_msgs::msg::String>(
-        "CS/nav_shutdown_cmds", 1, std::bind(&CmdvelManager::callback_shutdown, this, std::placeholders::_1));
-
-
       sub_nav_nodes_state = this->create_subscription<std_msgs::msg::String>(
         "NAV/nav_auto_state", 1, std::bind(&CmdvelManager::callback_nav_auto_state, this, std::placeholders::_1));
 
-
       sub_mode_nav = this->create_subscription<std_msgs::msg::String>(
         "/ROVER/NAV_mode", 1, std::bind(&CmdvelManager::callback_mode_nav, this, std::placeholders::_1));
-
-      destroy_sub_ = this->create_subscription<std_msgs::msg::String>("ROVER/NAV_status", rclcpp::QoS(rclcpp::KeepLast(1)), std::bind(&CmdvelManager::destroy_callback, this, std::placeholders::_1));
-
-
     }
 
 
   private:
-    void callback_shutdown(std_msgs::msg::String::SharedPtr msg)
-    {
-        if (msg->data == "NAV_SHUTDOWN")   
-        {
-            throw std::runtime_error("Shutdown requested");
-        }          
-    }
-
 
     void callback_mode_nav(std_msgs::msg::String::SharedPtr msg)
     {
@@ -145,11 +126,6 @@ class CmdvelManager : public rclcpp::Node
       }
     }
 
-      void destroy_callback(const std_msgs::msg::String::SharedPtr msg)
-    {
-        if(msg->data == "abort") rclcpp::shutdown();
-    }
-
     void callback_cmd_vel_auto(const geometry_msgs::msg::Twist::SharedPtr msg)
     {
 

wheels_control/src/Displacement_cmds_node.cpp

@@ -94,17 +94,12 @@ class DisplacementCmds : public rclcpp::Node
     sub_topic_absolute_encoders = this->create_subscription<custom_msg::msg::Wheelstatus>(
         "/NAV/absolute_encoders", 1, std::bind(&DisplacementCmds::callback_absolute_encoders, this, std::placeholders::_1));
 
-    sub_cmds_shutdown = this->create_subscription<std_msgs::msg::String>(
-        "/ROVER/NAV_status", 1, std::bind(&DisplacementCmds::callback_abort, this, std::placeholders::_1));
-
     sub_cmd_vel = this->create_subscription<geometry_msgs::msg::Twist>(
         "/NAV/cmd_vel_final", 1, std::bind(&DisplacementCmds::callback_cmd_vel, this, std::placeholders::_1));
-
+    /*
     sub_kinematic_state = this->create_subscription<std_msgs::msg::String>(
         "/ROVER/NAV_kinematic", 1, std::bind(&DisplacementCmds::callback_mode_kinematic, this, std::placeholders::_1));
-
-    destroy_sub_ = this->create_subscription<std_msgs::msg::String>("ROVER/NAV_status", rclcpp::QoS(rclcpp::KeepLast(1)), std::bind(&DisplacementCmds::destroy_callback, this, std::placeholders::_1));
-
+    */
     wheels_angle_for_rotation = get_wheels_angle_inc_for_rotation(); // unit: increment - value around 8 300
     wheels_angle_for_rotation_with_translation = (20 * (pow(2, 16))) / (360);
 
@@ -120,14 +115,6 @@ class DisplacementCmds : public rclcpp::Node
   bool go_left = false;
   bool go_right = false;
 
-  void callback_abort(std_msgs::msg::String::SharedPtr msg)
-  {
-    if (msg->data == "abort")
-    {
-      throw std::runtime_error("Shutdown requested");
-    }
-  }
-
   void callback_mode_kinematic(std_msgs::msg::String::SharedPtr msg)
   {
     if (msg->data == "normal")
@@ -236,12 +223,6 @@ class DisplacementCmds : public rclcpp::Node
     }
   }
 
-  void destroy_callback(const std_msgs::msg::String::SharedPtr msg)
-  {
-    if (msg->data == "abort")
-      rclcpp::shutdown();
-  }
-
   void callback_absolute_encoders(const custom_msg::msg::Wheelstatus::SharedPtr msg)
   {
     /*Update the motors position*/
@@ -277,7 +258,6 @@ class DisplacementCmds : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr sub_cs_gamepad;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_cmds_shutdown;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_kinematic_state;
-  rclcpp::Subscription<custom_msg::msg::Wheelstatus>::SharedPtr sub_topic_absolute_encoders;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr destroy_sub_;
 };