<launch>
	<!--
  <arg name="REMOTE_TF" default="/tf_low_frequency"/>
-->
  <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_JOINT_INTERFACE" default="true" />

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

  <!-- 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/hrp2w_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"/>
    
    <node pkg="jsk_interactive_marker" type="interactive_marker_interface" name="jsk_interactive_marker_manipulation" output="screen">
      <param name="use_center_sphere" type="boolean" value="true"/>
      <param name="change_using_ik_menu" type="boolean" value="true"/>
      <param name="base_frame" type="string" value="/BODY"/>
      <param name="move_menu" type="boolean" value="true"/>
      <param name="grasp_menu" type="boolean" value="true"/>
      <param name="use_head_marker" type="boolean" value="false"/>
      <param name="stop_grasp_menu" type="boolean" value="true"/>
      <param name="set_origin_menu" type="boolean" value="true"/>
      <param name="set_origin_to_rhand_menu" type="boolean" value="true"/>
      <param name="set_origin_to_lhand_menu" type="boolean" value="true"/>
      <param name="reset_marker_pos_menu" type="boolean" value="true"/>
      <param name="select_arm_menu" type="boolean" value="true"/>
      <param name="ik_mode_menu" type="boolean" value="true"/>
      <param name="use_torso_menu" type="boolean" value="true"/>
      <param name="force_mode_menu" type="boolean" value="false"/>
      <param name="publish_marker_menu" type="boolean" value="true"/>
    </node>


    <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"/>
      <param name="use_ik_server" value="false"/>
    </node>

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

  <!-- for interactive controller -->
  <node if="$(arg LAUNCH_JOINT_INTERFACE)"
        pkg="roseus" type="roseus" name="move_joint_interface_for_im"
	args="$(find jsk_interactive)/euslisp/move-joint-interface-robot.l">
    <param name="robot" value="$(arg ROBOT)"/>
  </node>

</launch>
