update_init

Author: shakuevda

src/stingray_core_control/stingray_core_control/stingray_core_control_node.py

@@ -36,114 +36,141 @@
 class StingrayCoreControlNode(Node):
     def __init__(self):
         super().__init__('stingray_core_control_node')
+        self._init_node_params()
+        self._init_topics()
+        self._init_axes_and_thrusters()
+        self._init_mixer()
+        self._init_controllers()
+        self._init_state()
+        self._init_axis_controllers()
+        self._init_subscriptions()
+        self._init_publishers()
+        self._init_param_callbacks()
+        self._init_timer()
 
-        # --- параметры ---
+        self.get_logger().info(
+            f"Started at {self.rate_hz} Hz | axes={self.axes} | thrusters={len(self.thrusters)}"
+        )
+
+    def control_loop(self):
+        now = time.time()
+        dt = now - self.last_time if self.last_time is not None else 0.0
+        self.last_time = now
+        self.last_dt = dt
+
+        # === 1. Определяем управляющие воздействия ===
+        u: dict[str, float] = {}
+
+        for axis in self.axes:
+            if axis in self.axis_ctrl and self.control.enabled[axis]:
+                u[axis] = self.axis_ctrl[axis].compute(
+                    target=self.control.impact[axis],
+                    dt=self.last_dt,
+                )
+            else:
+                u[axis] = self.control.impact[axis]
+
+        # === 2. Преобразуем в команды thrusters ===
+        control_list = [u[a] for a in self.axes]
+        thruster_cmds = self.mixer.mix_from_list(control_list)
+
+        # === 3. Публикуем thruster_cmd ===
+        msg = UInt8MultiArray()
+        msg.data = thruster_cmds
+        self.pub_thruster_cmd.publish(msg)
+
+        # === 4. Публикуем ориентацию ===
+        self.pub_yaw.publish(Float64(data=self.imu.yaw))
+        self.pub_pitch.publish(Float64(data=self.imu.pitch))
+        self.pub_roll.publish(Float64(data=self.imu.roll))
+
+    def normalize_angle_deg(self, angle_deg):
+        """Нормализует угол в градусах в диапазон [-180, 180)."""
+        a = (angle_deg + 180.0) % 360.0 - 180.0
+        return a
+
+    def _init_node_params(self):
         self.declare_parameter('rate_hz', 100.0)
-        self.rate_hz = float(self.get_parameter(
-            'rate_hz').get_parameter_value().double_value)
-        
+        self.rate_hz = float(self.get_parameter('rate_hz').value)
+
         self.declare_parameter('flag_setup_feedback_speed', False)
-        self.flag_setup_feedback_speed = self.get_parameter(
-            'flag_setup_feedback_speed').get_parameter_value().bool_value
-
-        self.declare_parameter('topic_imu_angular', '/vectornav/raw/common')
-        self.declare_parameter('topic_imu_linear_accel',
-                               '/vectornav/imu_accel')
-        self.declare_parameter('topic_imu_angular_rate', '/vectornav/imu')
-        self.declare_parameter('topic_loop_flags', '/control/loop_flags')
-        self.declare_parameter('topic_pressure_sensor', '/sensors/pressure')
-        self.declare_parameter('topic_control_data', '/control/data')
-        self.declare_parameter('vectornav_yaw_offset_deg', 0.0)
-
-        self.topic_imu_angular = self.get_parameter(
-            'topic_imu_angular').get_parameter_value().string_value
-        self.topic_imu_linear_accel = self.get_parameter(
-            'topic_imu_linear_accel').get_parameter_value().string_value
-        self.topic_imu_angular_rate = self.get_parameter(
-            'topic_imu_angular_rate').get_parameter_value().string_value
-        self.topic_loop_flags = self.get_parameter(
-            'topic_loop_flags').get_parameter_value().string_value
-        self.topic_pressure_sensor = self.get_parameter(
-            'topic_pressure_sensor').get_parameter_value().string_value
-        self.topic_control_data = self.get_parameter(
-            'topic_control_data').get_parameter_value().string_value
-        self.vectornav_yaw_offset = float(self.get_parameter(
-            'vectornav_yaw_offset_deg').get_parameter_value().double_value)
-        
-        # --- yaw zeroing ---
-        self.yaw_zero_offset = 0.0
-        self.imu_yaw_raw = 0.0
+        self.flag_setup_feedback_speed = bool(self.get_parameter('flag_setup_feedback_speed').value)
 
-        self.declare_parameter('topic_zero_yaw', '/imu/zero_yaw')
-        self.topic_zero_yaw = self.get_parameter(
-            'topic_zero_yaw').get_parameter_value().string_value
+        self.declare_parameter('debug_publish', False)
 
-        self.sub_zero_yaw = self.create_subscription(
-            Bool, self.topic_zero_yaw, self.zero_yaw_callback, 10)
 
-        
-        if not self.has_parameter('thrusters'):
-            self.declare_parameter('thrusters', ["",""])
+    def _init_topics(self):
+        defaults = {
+            'topic_imu_angular': '/vectornav/raw/common',
+            'topic_imu_linear_accel': '/vectornav/imu_accel',
+            'topic_imu_angular_rate': '/vectornav/imu',
+            'topic_loop_flags': '/control/loop_flags',
+            'topic_pressure_sensor': '/sensors/pressure',
+            'topic_control_data': '/control/data',
+            'topic_zero_yaw': '/imu/zero_yaw',
+        }
 
-        self.thrusters = list(self.get_parameter('thrusters').get_parameter_value().string_array_value)
-        
+        for name, default in defaults.items():
+            self.declare_parameter(name, default)
+            setattr(self, name, self.get_parameter(name).value)
+
+    def _init_axes_and_thrusters(self):
         self.declare_parameter('axes', ["surge", "sway", "heave", "roll", "pitch", "yaw"])
-        self.axes = list(self.get_parameter('axes').get_parameter_value().string_array_value)
+        self.axes = list(self.get_parameter('axes').value)
 
+        self.declare_parameter('thrusters', [])
+        self.thrusters = list(self.get_parameter('thrusters').value)
+
+    def _init_mixer(self):
         coeffs = {}
-        for t in self.thrusters:
-            row = []
-            for a in self.axes:
-                param_name = f"{t}_{a}"
-                if not self.has_parameter(param_name):
-                    self.declare_parameter(param_name, 0)
-                val = float(self.get_parameter(param_name).value)
-                row.append(val)
-            coeffs[t] = row
-
-        # create pure mixer
-        self.mixer = ThrusterMixer(self.thrusters, self.axes, coeffs)
-        self.get_logger().info(f"ThrusterMixer initialized: thrusters={self.thrusters}, axes={self.axes}, coeffs= {coeffs}")
 
-        # subscribe to parameter changes to update coeffs dynamically
+        for thr in self.thrusters:
+            row = []
+            for axis in self.axes:
+                pname = f"{thr}_{axis}"
+                if not self.has_parameter(pname):
+                    self.declare_parameter(pname, 0.0)
+                row.append(float(self.get_parameter(pname).value))
+            coeffs[thr] = row
 
-        self.controllers = {}
+        self.mixer = ThrusterMixer(self.thrusters, self.axes, coeffs)
 
+    def _init_controllers(self):
         axis_class_map = {
             'yaw': YawController,
             'pitch': PitchController,
             'roll': RollController,
             'heave': DepthController,
             'surge': SurgeController,
-            'sway': SwayController
+            'sway': SwayController,
         }
 
-
+        self.controllers = {}
         self.param_keys = ["K_p", "K_1", "K_2", "K_i", "I_min", "I_max", "out_sat", "ap_K", "ap_T"]
 
-        # внутри ноды
         for axis in self.axes:
-            # Получаем список ключей для оси
-            self.declare_parameter(axis, ["K_p", "K_1", "K_2", "K_i", "I_min", "I_max", "out_sat", "ap_K", "ap_T"])
-
-            # Загружаем параметры
-            params_dict = {}
+            params = {}
             for key in self.param_keys:
-                param_name = f'controllers.{axis}.{key}'
-                self.declare_parameter(param_name, 0.0)
-                params_dict[key] = self.get_parameter(param_name).value
-
-            # Инициализируем контроллер конкретного класса
-            ctrl_class = axis_class_map[axis]  # всегда берём нужный класс
-            self.controllers[axis] = ctrl_class(**params_dict)
+                pname = f"controllers.{axis}.{key}"
+                self.declare_parameter(pname, 0.0)
+                params[key] = float(self.get_parameter(pname).value)
 
+            self.controllers[axis] = axis_class_map[axis](**params)
+        # self.controllers["yaw"].set_debug_hook(self.debug_cb)
+        # self.controllers["heave"].set_debug_hook(self.debug_cb)
 
-        self.get_logger().info(f"Controllers initialized: {self.controllers}")
 
+    def _init_state(self):
         self.imu = ImuState()
         self.control = ControlState.from_axes(self.axes)
 
-        # --- Axis controllers (angular / linear split) ---
+        self.depth = 0.0
+
+        # yaw zeroing
+        self.yaw_zero_offset = 0.0
+        self.imu_yaw_raw = 0.0
+
+    def _init_axis_controllers(self):
         self.axis_ctrl: dict[str, AxisController] = {
             "yaw": AngularAxisController(
                 controller=self.controllers["yaw"],
@@ -160,114 +187,78 @@ def __init__(self):
             "heave": LinearAxisController(
                 controller=self.controllers["heave"],
                 pos_fn=lambda: self.depth,
-                vel_fn=lambda: 0.0,   # пока нет оценки вертикальной скорости
+                vel_fn=lambda: 0.0,
                 accel_fn=lambda: 0.0,
                 feedback_flag_fn=lambda: self.flag_setup_feedback_speed,
             ),
         }
 
+        # fallback для остальных осей
+        # for axis in self.axes:
+        #     if axis not in self.axis_ctrl:
+        #         self.axis_ctrl[axis] = PassthroughAxisController()
+
+    def _init_subscriptions(self):
         qos = QoSProfile(depth=10)
 
-        # --- подписки ---
         self.sub_imu_angular = self.create_subscription(
-            CommonGroup, "/vectornav/raw/common", self.imu_angular_callback, qos_profile_sensor_data)
+            CommonGroup, self.topic_imu_angular,
+            self.imu_angular_callback, qos_profile_sensor_data
+        )
+
         self.sub_imu_linear_accel = self.create_subscription(
-            Vector3, self.topic_imu_linear_accel, self.imu_linear_accel_callback, qos)
-        self.sub_imu_angular_rate = self.create_subscription(
-            Imu, self.topic_imu_angular_rate, self.imu_angular_rate_callback, qos)
-        
-        # --- my orientation publishers ---
-        self.pub_yaw   = self.create_publisher(Float64, '~/orientation/yaw', 10)
-        self.pub_pitch = self.create_publisher(Float64, '~/orientation/pitch', 10)
-        self.pub_roll  = self.create_publisher(Float64, '~/orientation/roll', 10)
+            Vector3, self.topic_imu_linear_accel,
+            self.imu_linear_accel_callback, qos
+        )
 
+        self.sub_imu_angular_rate = self.create_subscription(
+            Imu, self.topic_imu_angular_rate,
+            self.imu_angular_rate_callback, qos
+        )
 
-        # loop flags — используем UInt8 (битфлаги)
         self.sub_control_mode_flags = self.create_subscription(
-            UInt8, self.topic_loop_flags, self.control_mode_flags_callback, qos)
+            UInt8, self.topic_loop_flags,
+            self.control_mode_flags_callback, qos
+        )
 
-        # pressure: обычно Float32
         self.sub_pressure_sensor = self.create_subscription(
-            Float32, self.topic_pressure_sensor, self.pressure_sensor_callback, qos)
+            Float32, self.topic_pressure_sensor,
+            self.pressure_sensor_callback, qos
+        )
 
-        # control data (входные "impact" команды)
         self.sub_control_data = self.create_subscription(
-            Twist, self.topic_control_data, self.control_data_callback, qos)
+            Twist, self.topic_control_data,
+            self.control_data_callback, qos
+        )
+
+        self.sub_zero_yaw = self.create_subscription(
+            Bool, self.topic_zero_yaw,
+            self.zero_yaw_callback, 10
+        )
 
-        # паблишер
+    def _init_publishers(self):
         self.pub_thruster_cmd = self.create_publisher(
-            UInt8MultiArray, '/thruster/cmd', qos)
+            UInt8MultiArray, '/thruster/cmd', 10
+        )
 
-        self.depth = 0.0
-        self.yaw_ctrl = self.controllers["yaw"]
-        self.depth_ctrl = self.controllers["heave"]
-        self.pitch_ctrl = self.controllers["pitch"]
+        self.pub_yaw = self.create_publisher(Float64, '~/orientation/yaw', 10)
+        self.pub_pitch = self.create_publisher(Float64, '~/orientation/pitch', 10)
+        self.pub_roll = self.create_publisher(Float64, '~/orientation/roll', 10)
 
-        self.declare_parameter('debug_publish', False)
-        self.pub_err_position   = self.create_publisher(Float64, "~/debug/err_position", 10)
-        self.pub_output_pi      = self.create_publisher(Float64, "~/debug/output_pi", 10)
+        self.pub_err_position = self.create_publisher(Float64, "~/debug/err_position", 10)
+        self.pub_output_pi = self.create_publisher(Float64, "~/debug/output_pi", 10)
         self.pub_feedback_speed = self.create_publisher(Float64, "~/debug/feedback_speed", 10)
-        self.pub_measurement_rate    = self.create_publisher(Float64, "~/debug/measurement_rate", 10)
-        self.pub_out            = self.create_publisher(Float64, "~/debug/out", 10)
-        self.yaw_ctrl.set_debug_hook(self.debug_cb)
-        self.depth_ctrl.set_debug_hook(self.debug_cb)
-
-
+        self.pub_measurement_rate = self.create_publisher(Float64, "~/debug/measurement_rate", 10)
+        self.pub_out = self.create_publisher(Float64, "~/debug/out", 10)
+    
+    def _init_param_callbacks(self):
         self.add_on_set_parameters_callback(self._on_params_changed)
 
-        # таймер
+    def _init_timer(self):
         self.last_time = time.time()
         self.timer = self.create_timer(1.0 / self.rate_hz, self.control_loop)
 
-        self._last_status_log = 0.0
-        self._status_log_interval = 1.0  # сек
-
-        self.get_logger().info(
-            f"StingrayCoreControlNode started at {self.rate_hz} Hz")
-
-    def normalize_angle_deg(self, angle_deg):
-        """Нормализует угол в градусах в диапазон [-180, 180)."""
-        a = (angle_deg + 180.0) % 360.0 - 180.0
-        return a
-
-    def control_loop(self):
-        now = time.time()
-        dt = now - self.last_time if self.last_time is not None else 0.0
-        self.last_time = now
-        self.last_dt = dt
-
-        # === 1. Определяем управляющие воздействия ===
-        u: dict[str, float] = {}
-
-        for axis in self.axes:
-            if axis in self.axis_ctrl and self.control.enabled[axis]:
-                u[axis] = self.axis_ctrl[axis].compute(
-                    target=self.control.impact[axis],
-                    dt=self.last_dt,
-                )
-            else:
-                u[axis] = self.control.impact[axis]
-
-        # === 2. Преобразуем в команды thrusters ===
-        control_list = [u[a] for a in self.axes]
-        thruster_cmds = self.mixer.mix_from_list(control_list)
-
-        # === 3. Публикуем UInt8MultiArray ===
-        msg = UInt8MultiArray()
-        msg.data = thruster_cmds
-        self.pub_thruster_cmd.publish(msg)
-
-        # === 4. Публикуем ориентацию ===
-        self.pub_yaw.publish(Float64(data=self.imu.yaw))
-        self.pub_pitch.publish(Float64(data=self.imu.pitch))
-        self.pub_roll.publish(Float64(data=self.imu.roll))
-
 
-        # Логируем статус 1 раз в секунду, а не каждый тик
-        if now - self._last_status_log > self._status_log_interval:
-            # self.get_logger().info(f"Thrusters command: {thruster_cmds}")
-            # self.get_logger().debug(f"Control loop tick: dt={dt:.6f} s")
-            self._last_status_log = now
 
     # --- Колбэки ---
     def imu_angular_callback(self, msg: CommonGroup):