シミュレータで舵角の遅れ系を再現

simulator/CMakeLists.txt

@@ -30,6 +30,8 @@ install(
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(FILES scripts/steering_actuator_model.py DESTINATION lib/${PROJECT_NAME})
+
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
 
 ament_package()

simulator/launch/gazebo_ignition.launch.py

@@ -72,6 +72,14 @@ def generate_launch_description():
             'input_topic': '/cmd_vel',
             'output_topic': '/cmd_vel_twist',
             'wheel_base': 0.8,
+            'use_sim_time': True,
+            'update_rate': 100.0,
+            'steer_dead_time': 0.04,
+            'steer_tau': 0.12,
+            'steer_gain': 0.9,
+            'steer_rate_max_deg': 80.0,
+            'a_lat_max': 4.0,
+            'stop_velocity': 0.1,
         }]
     )
 

simulator/scripts/steered_to_twist.py

@@ -9,6 +9,8 @@
 from std_msgs.msg import Float64MultiArray
 from steered_drive_msg.msg import SteeredDrive
 
+from steering_actuator_model import SteeringActuatorModel, SteeringActuatorParams
+
 
 class SteeredToTwist(Node):
     def __init__(self) -> None:
@@ -18,37 +20,69 @@ def __init__(self) -> None:
         self.declare_parameter('input_topic', '/cmd_vel')
         self.declare_parameter('output_topic', '/cmd_vel_twist')
         self.declare_parameter('caster_topic', '/cmd_caster')
+        self.declare_parameter('caster_data_topic', '/caster_data')
+        self.declare_parameter('update_rate', 100.0)
+        self.declare_parameter('steer_dead_time', 0.04)
+        self.declare_parameter('steer_tau', 0.12)
+        self.declare_parameter('steer_gain', 0.9)
+        self.declare_parameter('steer_rate_max_deg', 80.0)
+        self.declare_parameter('a_lat_max', 4.0)
+        self.declare_parameter('stop_velocity', 0.1)
 
         self._wheel_base = float(self.get_parameter('wheel_base').get_parameter_value().double_value)
         input_topic = self.get_parameter('input_topic').get_parameter_value().string_value
         output_topic = self.get_parameter('output_topic').get_parameter_value().string_value
         caster_topic = self.get_parameter('caster_topic').get_parameter_value().string_value
+        caster_data_topic = self.get_parameter('caster_data_topic').value
 
         if self._wheel_base <= 0.0:
             self.get_logger().error('wheel_base must be positive; forcing 1.0')
             self._wheel_base = 1.0
 
         self.get_logger().info(f'Using wheel_base: {self._wheel_base:.4f} m')
 
+        self._model = SteeringActuatorModel(SteeringActuatorParams(
+            dead_time=self.get_parameter('steer_dead_time').value,
+            tau=self.get_parameter('steer_tau').value,
+            gain=self.get_parameter('steer_gain').value,
+            rate_max=math.radians(self.get_parameter('steer_rate_max_deg').value),
+            a_lat_max=self.get_parameter('a_lat_max').value,
+            wheel_base=self._wheel_base,
+            stop_velocity=self.get_parameter('stop_velocity').value,
+        ))
+        self._cmd_velocity = 0.0
+        self._cmd_steering = 0.0
+
         self._pub = self.create_publisher(Twist, output_topic, 10)
         self._caster_pub = self.create_publisher(Float64MultiArray, caster_topic, 10)
+        self._caster_data_pub = self.create_publisher(Float64MultiArray, caster_data_topic, 10)
         self._sub = self.create_subscription(SteeredDrive, input_topic, self.cmd_callback, 10)
+        self._timer = self.create_timer(1.0 / self.get_parameter('update_rate').value, self._on_timer)
+
+    def _now(self) -> float:
+        return self.get_clock().now().nanoseconds * 1e-9
 
     def cmd_callback(self, msg: SteeredDrive) -> None:
-        linear_vel = float(msg.velocity)
-        steering_angle = float(msg.steering_angle)
+        self._cmd_velocity = float(msg.velocity)
+        self._cmd_steering = float(msg.steering_angle)
+        self._model.set_command(self._now(), self._cmd_steering)
 
-        angular_vel = (linear_vel * math.tan(steering_angle)) / self._wheel_base
+    def _on_timer(self) -> None:
+        delta_eff = self._model.update(self._now(), self._cmd_velocity)
 
         twist = Twist()
-        twist.linear.x = linear_vel
-        twist.angular.z = angular_vel
+        twist.linear.x = self._cmd_velocity
+        twist.angular.z = self._model.yaw_rate(self._cmd_velocity, delta_eff)
         self._pub.publish(twist)
 
         caster_cmd = Float64MultiArray()
-        caster_cmd.data = [steering_angle*-1.0]
+        caster_cmd.data = [delta_eff * -1.0]
         self._caster_pub.publish(caster_cmd)
 
+        caster_data = Float64MultiArray()
+        caster_data.data = [self._cmd_steering, delta_eff, 0.0]
+        self._caster_data_pub.publish(caster_data)
+
 
 def main() -> None:
     rclpy.init()

simulator/scripts/steering_actuator_model.py

@@ -0,0 +1,47 @@
+import math
+from collections import deque
+from dataclasses import dataclass
+
+
+@dataclass
+class SteeringActuatorParams:
+    dead_time: float = 0.04
+    tau: float = 0.12
+    gain: float = 0.9
+    rate_max: float = math.radians(80.0)
+    a_lat_max: float = 4.0
+    wheel_base: float = 0.8
+    stop_velocity: float = 0.1
+    v_min: float = 0.5
+
+
+class SteeringActuatorModel:
+    def __init__(self, params: SteeringActuatorParams) -> None:
+        self._params = params
+        self._history = deque()
+        self._held_cmd = 0.0
+        self._rate_state = 0.0
+        self._filt = 0.0
+        self._t_prev = None
+
+    def set_command(self, t: float, steering_cmd: float) -> None:
+        self._history.append((t, steering_cmd))
+
+    def update(self, t: float, velocity: float) -> float:
+        p = self._params
+        if self._t_prev is None:
+            self._t_prev = t
+            return self._filt
+        dt = t - self._t_prev
+        self._t_prev = t
+        while self._history and self._history[0][0] <= t - p.dead_time:
+            self._held_cmd = self._history.popleft()[1]
+        target = 0.0 if abs(velocity) < p.stop_velocity else p.gain * self._held_cmd
+        self._rate_state += max(-p.rate_max * dt, min(p.rate_max * dt, target - self._rate_state))
+        self._filt += dt / (p.tau + dt) * (self._rate_state - self._filt)
+        return self._filt
+
+    def yaw_rate(self, velocity: float, steering: float) -> float:
+        p = self._params
+        w_max = p.a_lat_max / max(abs(velocity), p.v_min)
+        return max(-w_max, min(w_max, velocity * math.tan(steering) / p.wheel_base))