<launch>
    <arg name="wname" default="stairs"/>
    <arg name="rname" default="cyberdog"/>
    <param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
    <arg name="robot_path" value="(find $(arg rname)_description)"/>
    <arg name="dollar" value="$"/>

    <arg name="paused" default="true"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>
    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
    <arg name="user_debug" default="false"/>
    
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- Load the URDF into the ROS Parameter Server -->
    <param name="robot_description" textfile="$(find cyberdog_description)/urdf/cyberdog_description.urdf"/>

    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
    <!-- Set trunk and joint positions at startup -->
    <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
          args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description -unpause"/>

    <!-- Load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>

    <!-- <rosparam param="/a1_gazebo/joint_state_controller/publish_rate">5000</rosparam> -->

    <!-- load the controllers -->
    <node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
          output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
          FL_hip_controller FL_thigh_controller FL_calf_controller
          FR_hip_controller FR_thigh_controller FR_calf_controller
          RL_hip_controller RL_thigh_controller RL_calf_controller
          RR_hip_controller RR_thigh_controller RR_calf_controller "/>

    <!-- convert joint states to TF transforms for rviz, etc -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
          respawn="false" output="screen">
        <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
    </node>

    <!-- <node pkg="unitree_gazebo" type="servo" name="servo" required="true" output="screen"/> -->

    <!-- load the parameter unitree_controller -->
    <include file="$(find unitree_controller)/launch/set_ctrl.launch">
        <arg name="rname" value="$(arg rname)"/>
    </include>

</launch>
