sr_hardware_control_loop.launch

<launch>
  <arg name="debug" default="false"/>
  <arg name="robot_state_pub_frequency" default="250"/>
  <arg name="joint_state_pub_frequency" default="125"/>

  <!-- ROBOT CONFIG-->
  <arg name="robot_model" default="ur10"/>
  <arg name="side" default="right"/>
  <arg name="side_letter" value="$(eval side[0])"/>
  <arg name="hand_id" value="$(arg side_letter)h"/>
  <arg name="arm_prefix" value="$(arg side_letter)a"/>
  <!-- Whether there is an arm. -->
  <arg name="arm" default="true"/>
  <!-- Whether there is a hand. -->
  <arg name="hand" default="true"/>
  <!-- Whether to run arm controllers. -->
  <arg name="arm_ctrl" default="$(arg arm)"/>
  <!-- Whether to run hand controllers. -->
  <arg name="hand_ctrl" default="$(arg hand)"/>

  <!-- Robot description -->
  <arg if="$(eval arg('hand') and arg('arm'))" name="robot_description" default="$(find sr_multi_description)/urdf/$(arg side)_srhand_ur10_bt_sp_biotacs.urdf.xacro"/>
  <arg if="$(eval arg('hand') and arg('arm'))" name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/$(arg side)_sh_$(arg robot_model).yaml"/>
  <arg if="$(eval not arg('hand') and arg('arm'))" name="robot_description" default="$(find sr_box_ur10_moveit_config)/config/$(arg side_letter)a_$(arg robot_model)_with_box.urdf.xacro"/>
  <arg if="$(eval not arg('hand') and arg('arm'))" name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/$(arg side)_sh_$(arg robot_model).yaml"/>
  <arg if="$(eval arg('hand') and arg('hand_id')=='rh' and not arg('arm'))" name="robot_description" default="$(find sr_description)/robots/shadowhand_motor_plus.urdf.xacro"/>
  <arg if="$(eval arg('hand') and arg('hand_id')=='lh' and not arg('arm'))" name="robot_description" default="$(find sr_description)/robots/shadowhand_left_motor_plus.urdf.xacro"/>
  <arg if="$(eval arg('hand') and not arg('arm'))" name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/$(arg side)_sh.yaml"/>

  <!-- Robot config -->

  <!-- HANDS CONFIG-->
  <arg name="hand_serial" default="1082"/>
  <arg name="mapping_path" default="$(find sr_edc_launch)/mappings/default_mappings/$(arg hand_id)_E_v3.yaml"/>
  <arg name="eth_port" default="$(optenv ETHERCAT_PORT enp2s0)"/>
  <arg name="pwm_control" default="true"/>
  <!-- Set to true if you want to use grasp controller -->
  <arg name="grasp_controller" default="false"/>
  <arg name="hand_trajectory" default="true"/>

  <!-- ARMS CONFIG-->
  <arg name="initial_z" if="$(arg arm)" default="0.7551"/>
  <arg name="initial_z" unless="$(arg arm)" default="0.0"/>
  <arg name="arm_robot_hw" default="$(find sr_robot_launch)/config/$(arg side)_ur_arm_robot_hw.yaml"/>
  <arg name="force_torque_sensor_hw" default="$(find sr_robot_launch)/config/$(arg arm_prefix)_force_torque_controller.yaml"/>
  <arg name="include_wrist_in_arm_controller" default="$(arg arm_ctrl)"/>
  <arg name="arm_trajectory" default="true"/>
  <!-- Set to true to spawn the position controllers for the arm-->
  <arg name="arm_position" default="$(eval not arm_trajectory)"/>
  <arg name="arm_speed_scale" default="0.5"/>
  <arg if="$(arg arm)" name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml"/>
  <arg unless="$(arg arm)" name="kinematics_config" default=""/>
  <arg name="urcap_program_name" default="external_ctrl.urp"/>
  <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config)"/>

  <node pkg="rosservice" type="rosservice" name="set_speed_scale" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg arm_speed_scale)'"/>

  <!-- Logging local topics -->
  <group ns="control_box">
    <include file="$(find sr_logging_common)/launch/sr_rosbag_log.launch">
      <arg name="log_topics" value='-e ".*(controller|debug_etherCAT_data).*" /joint_states /diagnostics /mechanism_statistics'/>
      <arg name="log_bag_prefix" value="sr_hardware_control"/>
      <arg name="log_directory" value="$(optenv HOME)/.ros/log"/>
    </include>
  </group>

  <!-- So that the driver can check we're loading the right calibration -->
  <rosparam if="$(arg arm)" command="load" file="$(arg kinematics_config)" ns="$(arg arm_prefix)_sr_ur_robot_hw"/>

  <include file="$(find sr_robot_launch)/launch/unimanual_controller_stopper.launch">
    <arg name="side" value="$(arg side)"/>
  </include>

  <!-- Default hand controller groups -->
  <arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>
  <arg if="$(eval hand_trajectory and not grasp_controller)" name="hand_controller_group" default="trajectory"/>
  <arg if="$(eval not hand_trajectory and not grasp_controller)" name="hand_controller_group" default="position"/>

  <!-- Controller -->
  <group if="$(arg hand)">
    <!-- Launch rosparam for payload if we use hand. If we only use hand and no arm the extra values wont be used-->
    <rosparam file="$(arg robot_config_file)"/>
    <!-- HAND (N.B. Arm robot harware is implicitly started here if ra_sr_ur_robot_hw is present in param /robot_hardware-->
    <include file="$(find sr_edc_launch)/sr_edc_ros_control.launch">
      <arg name="define_robot_hardware" value="false"/>
      <arg name="hand_robot_hardware_name" value="unique_robot_hw"/>
      <arg name="debug" value="$(arg debug)"/>
      <arg name="eth_port" value="$(arg eth_port)"/>
      <arg name="load_robot_description_command" value="$(arg load_robot_description_command)"/>
      <arg name="pwm_control" value="$(arg pwm_control)"/>
      <arg name="hand_serial" value="$(arg hand_serial)"/>
      <arg name="hand_id" value="$(arg hand_id)"/>
      <arg name="mapping_path" value="$(arg mapping_path)"/>
      <arg name="robot_state_pub_frequency" value="$(arg robot_state_pub_frequency)"/>
      <arg name="joint_state_pub_frequency" value="$(arg joint_state_pub_frequency)"/>
      <arg name="initial_z" value="$(arg initial_z)"/>
    </include>
    <node if="$(arg hand_ctrl)" name="$(arg hand_id)_controller_spawner" pkg="sr_utilities" type="controller_spawner.py" output="screen">
      <param name="controller_group" value="$(arg hand_controller_group)"/>
      <param name="exclude_wrist" value="$(arg include_wrist_in_arm_controller)"/>
      <param name="wait_for" value ="calibrated"/>
    </node>
  </group>

  <group if="$(arg arm_ctrl)">
    <group if="$(arg hand_ctrl)">
      <rosparam if="$(eval arg('side') == 'right')">
        robot_hardware:
          - unique_robot_hw
          - ra_sr_ur_robot_hw
      </rosparam>
      <rosparam if="$(eval arg('side') == 'left')">
        robot_hardware:
          - unique_robot_hw
          - la_sr_ur_robot_hw
      </rosparam>
    </group>
    <node name="sr_ur_arm_unlock" pkg="sr_robot_launch" type="sr_ur_arm_unlock" output="screen">
      <param name="urcap_program_name" value="$(arg urcap_program_name)"/>
    </node>
    <rosparam unless="$(arg hand_ctrl)" param="robot_hardware" subst_value="True">[$(arg arm_prefix)_sr_ur_robot_hw]</rosparam>
    <rosparam command="load" file="$(arg arm_robot_hw)"/>
    <param name="$(arg arm_prefix)_sr_ur_robot_hw/speed_scale" type="double" value="$(arg arm_speed_scale)"/>
    <group if="$(eval 'e' in arg('robot_model'))">
      <rosparam command="load" file="$(arg force_torque_sensor_hw)"/>
      <node name="ft_sensor_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg arm_prefix)_force_torque_sensor_controller" />
    </group>
  </group>

  <rosparam unless="$(arg arm_ctrl)">
    robot_hardware:
      - unique_robot_hw
  </rosparam>

  <!-- These will be loaded if hand is false so UR10 with box will load instead. -->
  <group if="$(eval not arg('hand') and arg('arm'))">
    <include file="$(find ros_ethercat_model)/launch/joint_state_publisher.launch">
      <arg name="publish_rate" value="$(arg joint_state_pub_frequency)"/>
    </include>
    <node if="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="gdb -ex run -args"/>
    <node unless="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="ethercat_grant"/>
    <param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config)"/>
  </group>
  <!-- These will be loaded if arm and hand are enabled -->
  <!-- Trajectory mode -->
  <group if="$(arg arm_ctrl)">
    <group if="$(arg arm_trajectory)">
      <group if="$(arg hand_ctrl)">
        <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
        <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller_no_wrist.yaml"/>
      </group>
      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller_no_wrist.yaml"/>
      <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
    </group>
    <!-- Position mode -->
    <group if="$(arg arm_position)">
      <group if="$(arg hand_ctrl)">
        <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller.yaml"/>
        <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller_no_wrist.yaml"/>
      </group>
      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller_no_wrist.yaml"/>
      <node if="$(arg hand_ctrl)" name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
            args="--wait-for=/$(arg arm_prefix)_arm_ready $(arg arm_prefix)_group_position_controller"/>
      <node unless="$(arg hand_ctrl)" name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
            args="--wait-for=/$(arg arm_prefix)_arm_ready $(arg arm_prefix)_group_position_controller"/>
    </group>
  </group>
</launch>