<?xml version="1.0" ?>
<launch>
  <arg name="robot_ip" default="172.16.0.2" />
  <arg name="load_gripper" default="true" />

  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/urdfs/panda_arm_hand.urdf.xacro" />

  <!-- Panda control nodes -->
  <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
    <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
    <param name="robot_ip" value="$(arg robot_ip)" />
  </node>

  <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
    <arg name="robot_ip" value="$(arg robot_ip)" />
  </include>

  <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />

  <!-- State publisher -->
  <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>

  <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
    <rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
    <param name="rate" value="30"/>
  </node>

  <!-- Controllers -->
  <rosparam command="load" file="$(find panda_controllers)/config/controllers.yaml" />
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--stopped position_joint_trajectory_controller cartesian_velocity_controller"/>

  <!-- Camera -->
  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="enable_infra1" value="true" />
    <arg name="enable_pointcloud" value="true" />
    <arg name="publish_tf" value="false" />
  </include>
</launch>
