<launch>
  <!--
      <arg name="REMOTE_TF" default="/tf_low_frequency"/>
  -->
  <rosparam command="load" param="robot_description" file="$(find hrpsys_ros_bridge_tutorials)/models/STARO.urdf"/>

  <arg name="REMOTE_TF" default="/tf"/>
  <arg name="USE_INTERACTIVE_JOINT" default="true" />
  <arg name="USE_INTERACTIVE_ENDCOORDS" default="true" />
  <arg name="ROBOT_DESCRIPTION_REMOTE" default="robot_description" />
  <arg name="LAUNCH_DYNAMIC_TF_PUBLISHER" default="true" />
  <arg name="LAUNCH_ALL_IN_REMOTE" default="true"/>

  <arg name="ROBOT" default="staro" />

  <!-- joint marker -->
  <group if="$(arg USE_INTERACTIVE_JOINT)">
    <remap from="/tf" to="$(arg REMOTE_TF)"/>
    <remap from="/set_dynamic_tf" to="/dynamic_tf_publisher_remote/set_dynamic_tf"/>
    <remap from="/delete_tf" to="/dynamic_tf_publisher_remote/delete_tf"/>
    <remap from="/dissoc_tf" to="/dynamic_tf_publisher_remote/dissoc_tf"/>
    <remap from="/assoc_tf" to="/dynamic_tf_publisher_remote/assoc_tf"/>
    <remap from="/publish_tf" to="/dynamic_tf_publisher_remote/publish_tf"/>

    <node pkg="rviz" type="rviz" name="rviz_for_interactive_marker" args="-d $(find jsk_interactive_marker)/launch/atlas_joint_marker.rviz" respawn="true">
      <remap from="/robot_description" to="$(arg ROBOT_DESCRIPTION_REMOTE)"/>
    </node>

    <include file="$(find jsk_interactive_marker)/launch/dummy_camera.launch">
      <arg name="origin_tf" value="/moving_marker"/>
      <arg name="dummy_camera_name" value="dummy_camera_above"/>
      <arg name="tf_pose" value="0 0 1 1.5708 3.1416 0"/>
    </include>

    <include file="$(find jsk_interactive_marker)/launch/dummy_camera.launch">
      <arg name="origin_tf" value="/moving_marker"/>
      <arg name="dummy_camera_name" value="dummy_camera_front"/>
      <arg name="tf_pose" value="1 0 0 1.5708 0 -1.5708"/>
    </include>

    <include file="$(find jsk_interactive_marker)/launch/dummy_camera.launch">
      <arg name="origin_tf" value="/moving_marker"/>
      <arg name="dummy_camera_name" value="dummy_camera_side"/>
      <arg name="tf_pose" value="0 1 0 0 3.1416 1.5708"/>
    </include>

    <include file="$(find jsk_interactive_marker)/launch/urdf_model_markers.launch">
      <arg name="LAUNCH_DYNAMIC_TF_PUBLISHER" value="$(arg LAUNCH_DYNAMIC_TF_PUBLISHER)"/>
      <arg name="models" value="$(find jsk_interactive_marker)/launch/models/staro_description.yaml"/>
    </include>
  </group>

  <!-- hand marker -->
  <group if="$(arg USE_INTERACTIVE_ENDCOORDS)" >
    <remap from="/tf" to="$(arg REMOTE_TF)"/>
    <remap from="/set_dynamic_tf" to="/dynamic_tf_publisher_remote/set_dynamic_tf"/>
    <remap from="/dissoc_tf" to="/dynamic_tf_publisher_remote/dissoc_tf"/>
    <remap from="/delete_tf" to="/dynamic_tf_publisher_remote/delete_tf"/>
    <remap from="/assoc_tf" to="/dynamic_tf_publisher_remote/assoc_tf"/>
    <remap from="/publish_tf" to="/dynamic_tf_publisher_remote/publish_tf"/>

    <include file="$(find jsk_interactive_marker)/launch/staro_interactive_marker.launch" />

    <node pkg="roseus" type="roseus" name="ik_controller" args="$(find jsk_interactive_marker)/euslisp/ik-controller-main.l" output="screen">
      <param name="robot" value="$(arg ROBOT)"/>
      <param name="frame_id" value="BODY"/>
    </node>

    <!-- eus-ik-server -->
    <node pkg="roseus" type="roseus" name="staro_ik_server"
	  args="$(find jsk_ik_server)/euslisp/ik-server-impl/staro-ik-server.l"/>
  </group>

  <!-- for interactive controller -->
  <group if="$(arg LAUNCH_ALL_IN_REMOTE)">
    <node pkg="roseus" type="roseus" name="move_joint_interface_for_im"
          args="$(find jsk_interactive)/euslisp/move-joint-interface-robot.l">
      <param name="robot" value="staro"/>
    </node>
    <node pkg="topic_tools" type="throttle" name="joint_states_throttle"
          args="messages /joint_states 1.0 /robot/joint_states_decompressed"/>
  </group>
</launch>
