diff --git a/simulator/CMakeLists.txt b/simulator/CMakeLists.txt index e05eddf7..197308cf 100644 --- a/simulator/CMakeLists.txt +++ b/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() \ No newline at end of file diff --git a/simulator/launch/gazebo_ignition.launch.py b/simulator/launch/gazebo_ignition.launch.py index 22c88d98..5d7b6295 100644 --- a/simulator/launch/gazebo_ignition.launch.py +++ b/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, }] ) diff --git a/simulator/scripts/steered_to_twist.py b/simulator/scripts/steered_to_twist.py index f4908454..028e4660 100755 --- a/simulator/scripts/steered_to_twist.py +++ b/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,11 +20,20 @@ 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') @@ -30,25 +41,48 @@ def __init__(self) -> None: 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() diff --git a/simulator/scripts/steering_actuator_model.py b/simulator/scripts/steering_actuator_model.py new file mode 100644 index 00000000..210ec983 --- /dev/null +++ b/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))