from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():

    velocity_smoother = Node(
        package="nav2_velocity_smoother",
        name="root_velocity_smoother",
        executable="velocity_smoother",
        parameters=[
            # {'use_sim_time': use_sim_time},
            {"smoothing_frequency": 20.0},
            {"scale_velocities": False},
            {"feedback": "OPEN_LOOP"},
            {"max_velocity": [0.5, 0.0, 0.8]},
            {"min_velocity": [-0.5, 0.0, -0.8]},
            {"velocity_timeout": 0.5},
            {"max_accel": [0.25, 0.0, 1.2]},
            {"max_decel": [-0.5, 0.0, -1.2]},
        ],
        output="screen",
    )

    lifecycle_manager_velocity_smoother = Node(
        package="nav2_lifecycle_manager",
        executable="lifecycle_manager",
        name="lifecycle_manager_velocity_smoother",
        parameters=[
            {"autostart": True},
            {"node_names": ["root_velocity_smoother"]},
        ],
        output="screen",
    )

    ld = LaunchDescription()
    ld.add_action(velocity_smoother)
    ld.add_action(lifecycle_manager_velocity_smoother)
    return ld

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: .nan" 

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.5"