<launch>

  <!-- GDB Debug Option -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix"
           value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />

  <!-- Verbose Mode Option -->
  <arg name="info" default="$(arg debug)" />
  <arg unless="$(arg info)" name="command_args" value="" />
  <arg     if="$(arg info)" name="command_args" value="--debug" />

  <!-- move_group settings -->
  <arg name="pipeline" default="ompl" />
  <arg name="allow_trajectory_execution" default="true"/>
  <arg name="moveit_controller_manager" default="simple" />
  <arg name="fake_execution_type" default="interpolate"/>
  <arg name="max_safe_path_cost" default="1"/>
  <arg name="publish_monitored_planning_scene" default="true"/>

  <arg name="capabilities" default=""/>
  <arg name="disable_capabilities" default=""/>
  <!-- load these non-default MoveGroup capabilities (space seperated) -->
  <!--
  <arg name="capabilities" value="
                a_package/AwsomeMotionPlanningCapability
                another_package/GraspPlanningPipeline
                " />
  -->

  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
  <!--
  <arg name="disable_capabilities" value="
                move_group/MoveGroupKinematicsService
                move_group/ClearOctomapService
                " />
  -->

  <arg name="load_robot_description" default="false" />
  <!-- load URDF, SRDF and joint_limits configuration -->
  <include file="$(dirname)/planning_context.launch">
    <arg name="load_robot_description" value="$(arg load_robot_description)" />
  </include>

  <!-- Planning Pipelines -->
  <group ns="move_group/planning_pipelines">

    <!-- OMPL -->
    <include file="$(dirname)/planning_pipeline.launch.xml">
      <arg name="pipeline" value="ompl" />
    </include>

    <!-- CHOMP -->
    <include file="$(dirname)/planning_pipeline.launch.xml">
      <arg name="pipeline" value="chomp" />
    </include>

    <!-- Pilz Industrial Motion -->
    <include file="$(dirname)/planning_pipeline.launch.xml">
      <arg name="pipeline" value="pilz_industrial_motion_planner" />
    </include>

    <!-- Support custom planning pipeline -->
    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
             file="$(dirname)/planning_pipeline.launch.xml">
      <arg name="pipeline" value="$(arg pipeline)" />
    </include>
  </group>

  <!-- Trajectory Execution Functionality -->
  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
  </include>

  <!-- Sensors Functionality -->
  <include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_sensor_manager" value="a9" />
  </include>

  <!-- Start the actual move_group node/action server -->
  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
    <!-- Set the display variable, in case OpenGL code is used internally -->
    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />

    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
    <param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
    <param name="default_planning_pipeline" value="$(arg pipeline)" />
    <param name="capabilities" value="$(arg capabilities)" />
    <param name="disable_capabilities" value="$(arg disable_capabilities)" />

    <!-- do not copy dynamics information from /joint_states to internal robot monitoring
         default to false, because almost nothing in move_group relies on this information -->
    <param name="monitor_dynamics" value="false" />

    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
  </node>

</launch>
