UniversalRobots / Universal_Robots_ROS2_Driver

Universal Robots ROS2 driver supporting CB3 and e-Series
BSD 3-Clause "New" or "Revised" License
431 stars 223 forks source link

xacro files with comments are interpreted as YAML #1103

Closed katallen405 closed 1 month ago

katallen405 commented 1 month ago

Affected ROS2 Driver version(s)

2.4.10

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with realtime patch

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR E-series robot

Robot SW / URSim version(s)

all

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

xacro files can be interpret as xacro when using description_package

Issue details

When launching with a custom description package (to show gripper), it causes the following error: [ERROR] [launch]: Caught exception in launch (see debug for traceback): Unable to parse the value of parameter robot_description as yaml. If the parameter is meant to be a string, try wrapping it in launch_ros.parameter_descriptions.ParameterValue(value, value_type=str)

This ROS issue has a known bug and workaround, but it requires that you change something in the file: https://answers.ros.org/question/417369/caught-exception-in-launch-see-debug-for-traceback-unable-to-parse-the-value-of-parameter-robot_description-as-yaml/

/ur_control.launch.py https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/launch/ur_control.launch.py

Steps to Reproduce

xacro files with commented xacro lines are particularly prone to this issue

Expected Behavior

valid xacro should launch correctly

Actual Behavior

[ERROR] [launch]: Caught exception in launch (see debug for traceback): Unable to parse the value of parameter robot_description as yaml. If the parameter is meant to be a string, try wrapping it in launch_ros.parameter_descriptions.ParameterValue(value, value_type=str)

What did you observe? If possible please attach relevant information.

Workaround Suggestion

no workaround

Relevant log output

ros2 launch ur_robot_driver ur3e.launch.py robot_ip:=130.64.17.5 launch_rviz:=false description_package:=ceeorobot_description --debug
[DEBUG] [launch.launch_context]: emitting event synchronously: 'launch.events.IncludeLaunchDescription'
[INFO] [launch]: All log files can be found below /home/kat/.ros/log/2024-09-17-11-30-42-642663-ur3e-hub-151123
[INFO] [launch]: Default logging verbosity is set to DEBUG
[DEBUG] [launch]: processing event: '<launch.events.include_launch_description.IncludeLaunchDescription object at 0x7fe97b77e5c0>'
[DEBUG] [launch]: processing event: '<launch.events.include_launch_description.IncludeLaunchDescription object at 0x7fe97b77e5c0>' ✓ '<launch.event_handlers.on_include_launch_description.OnIncludeLaunchDescription object at 0x7fe97b5cb880>'
[DEBUG] [launch.launch_description_source]: Traceback (most recent call last):
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_source.py", line 68, in try_get_launch_description_without_context
    perform_substitutions(context, self.__location)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in perform_substitutions
    return ''.join([context.perform_substitution(sub) for sub in subs])
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in <listcomp>
    return ''.join([context.perform_substitution(sub) for sub in subs])
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_context.py", line 240, in perform_substitution
    return substitution.perform(self)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/substitutions/this_launch_file_dir.py", line 56, in perform
    raise SubstitutionFailure(
launch.substitutions.substitution_failure.SubstitutionFailure: ThisLaunchFileDir used outside of a launch file (in a script)

[DEBUG] [launch.launch_description_source]: Failed to load the launch file without a context: ThisLaunchFileDir used outside of a launch file (in a script)
Executing <Task finished name='Task-2' coro=<LaunchService._process_one_event() done, defined at /opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py:228> exception=TypeError('Unable to parse the value of parameter robot_description as yaml. If the parameter is meant to be a string, try wrapping it in launch_ros.parameter_descriptions.ParameterValue(value, value_type=str)') created at /opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py:318> took 0.215 seconds
[DEBUG] [launch]: An exception was raised in an async action/event
[DEBUG] [launch]: Traceback (most recent call last):
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_ros/utilities/evaluate_parameters.py", line 79, in evaluate_parameter_dict
    yaml_evaluated_value = yaml.safe_load(evaluated_value)
  File "/usr/lib/python3/dist-packages/yaml/__init__.py", line 162, in safe_load
    return load(stream, SafeLoader)
  File "/usr/lib/python3/dist-packages/yaml/__init__.py", line 114, in load
    return loader.get_single_data()
  File "/usr/lib/python3/dist-packages/yaml/constructor.py", line 49, in get_single_data
    node = self.get_single_node()
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 36, in get_single_node
    document = self.compose_document()
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 58, in compose_document
    self.get_event()
  File "/usr/lib/python3/dist-packages/yaml/parser.py", line 118, in get_event
    self.current_event = self.state()
  File "/usr/lib/python3/dist-packages/yaml/parser.py", line 193, in parse_document_end
    token = self.peek_token()
  File "/usr/lib/python3/dist-packages/yaml/scanner.py", line 129, in peek_token
    self.fetch_more_tokens()
  File "/usr/lib/python3/dist-packages/yaml/scanner.py", line 258, in fetch_more_tokens
    raise ScannerError("while scanning for the next token", None,
yaml.scanner.ScannerError: while scanning for the next token
found character '\t' that cannot start any token
  in "<unicode string>", line 8, column 1:
            Robotiq Hand-E gripper. -->
    ^

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 336, in run_async
    raise completed_tasks_exceptions[0]
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 230, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 250, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  [Previous line repeated 4 more times]
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_ros/actions/node.py", line 490, in execute
    self._perform_substitutions(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_ros/actions/node.py", line 445, in _perform_substitutions
    evaluated_parameters = evaluate_parameters(context, self.__parameters)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_ros/utilities/evaluate_parameters.py", line 164, in evaluate_parameters
    output_params.append(evaluate_parameter_dict(context, param))
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_ros/utilities/evaluate_parameters.py", line 81, in evaluate_parameter_dict
    raise TypeError(
TypeError: Unable to parse the value of parameter robot_description as yaml. If the parameter is meant to be a string, try wrapping it in launch_ros.parameter_descriptions.ParameterValue(value, value_type=str)

[ERROR] [launch]: Caught exception in launch (see debug for traceback): Unable to parse the value of parameter robot_description as yaml. If the parameter is meant to be a string, try wrapping it in launch_ros.parameter_descriptions.ParameterValue(value, value_type=str)
[DEBUG] [launch.launch_context]: emitting event: 'launch.events.Shutdown'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7fe97b5cb760>'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7fe97b5cb760>' ✓ '<launch.event_handlers.on_shutdown.OnShutdown object at 0x7fe97b5cbb80>'

Accept Public visibility

VinDp commented 1 month ago

Hi @katallen405, can you provide an example description?

agonzat commented 1 month ago

We can reproduce this as well on Humble.

Adding ParameterValue(robot_description_content, value_type=str) to all instances across the different packages/launchfiles seems to fix the issue.

fmauch commented 1 month ago

I was not aware of this problem, thanks for pointing that out.

katallen405 commented 1 month ago

I think this is the xacro file that is catching the issue. Thanks!

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <!--
    Based on the URDF file by Cristian C Beltran-Hernandez
  -->  
    <!-- If visualizing just the gripper with no arm, set the true, otherwise, set to false -->
    <xacro:property name="gripper_only" value="false"/>
    <xacro:property name="parent"/>

    <xacro:include filename="$(find robotiq_hande_description)/urdf/materials.xacro" />

  <xacro:macro name="robotiq_hande_gripper" params="
        name
        prefix
        parent
        *origin
        sim_isaac:=false
        use_fake_hardware:=false
        include_ros2_control:=true
        com_port:=/dev/ttyUSB0">

    <!-- ros2 control include -->
    <xacro:include filename="$(find robotiq_description)/urdf/robotiq_gripper.ros2_control.xacro" />
    <!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
    <xacro:if value="${include_ros2_control}">
    <xacro:robotiq_gripper_ros2_control
        name="${name}" prefix="${prefix}"
        sim_isaac="${sim_isaac}"
        use_fake_hardware="${use_fake_hardware}"
        com_port="${com_port}"/>
    </xacro:if>

    <!-- Robotiq Coupler -->
    <!--  + Height added by the coupler: 13.9mm -->
    <!--  + Reference frame: at the middle (6.95mm) -->

    <joint name="robotiq_coupler_joint" type="fixed">
      <origin xyz="0 0 0.00695" rpy="0 0 ${-pi/2.0}" />
      <parent link="${parent}"/>
      <child link="${prefix}_robotiq_coupler"/>
    </joint>

    <link name="${prefix}_robotiq_coupler">
      <inertial>
        <mass value="0.168" />
        <inertia 
          ixx="6.17674E-05" ixy="0.0" ixz="0.0" 
          iyy="6.17674E-05" iyz="0.0" 
          izz="1.18125E-04"
        />
      </inertial>

      <collision>
        <geometry>
          <cylinder length="0.0139" radius="0.0375"/>
        </geometry>
      </collision>

      <visual>
        <geometry>
          <mesh filename="package://robotiq_hande_description/meshes/coupler.dae" />
        </geometry>
        <material name="DarkGrey" />
      </visual>
    </link>

<!-- gripper body -->

    <joint name="${prefix}_robotiq_hande_base_joint" type="fixed">
      <origin rpy="0 0 0" xyz="0 0 0.055"/>
      <parent link="${prefix}_robotiq_coupler"/>
      <child link="${prefix}_hande_link"/>
    </joint>

    <link name="${prefix}_hande_link">
      <inertial>
        <mass value="0.86387"/>
        <inertia 
          ixx="0.001" ixy="0.0" ixz="0.0" 
          iyy="0.001" iyz="0.0" 
          izz="0.0006"
        />  
      </inertial>

      <collision>
        <geometry>
          <cylinder length="0.0988" radius="0.0375"/>
        </geometry>
      </collision>

      <visual>
        <origin rpy="0 -1.570796 -1.570796" xyz="0 0 -0.0016"/>
        <geometry>
          <mesh filename="package://robotiq_hande_description/meshes/hande.dae" scale="1.0 1.0 1.0"/>
        </geometry>
        <material name="DarkGrey"/>
      </visual>
    </link>

    <!-- gripper left finger -->

    <joint name="${prefix}_hande_left_finger_joint" type="prismatic">
      <origin rpy="0 0 0" xyz="0 -0.025 0.0345"/>
      <parent link="${prefix}_hande_link"/>
      <child link="${prefix}_hande_left_finger"/>
      <axis xyz="0 1 0"/>
      <limit effort="130" lower="0" upper="0.025" velocity="0.15"/>
    </joint>

    <link name="${prefix}_hande_left_finger">
      <inertial>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value="0.03804"/>
        <inertia
          ixx="1E-9" ixy="0.0" ixz="0.0" 
          iyy="1E-9" iyz="0.0" 
          izz="1E-9"
        />
      </inertial>

      <collision>
        <origin rpy="0 0 3.1415926" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://robotiq_hande_description/meshes/finger_collision.dae" scale="1.0 1.0 1.0"/>
        </geometry>
      </collision>

      <visual>
        <origin rpy="1.570796 0 3.1415926" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://robotiq_hande_description/meshes/finger.dae" scale="1.0 1.0 1.0"/>
        </geometry>
        <material name="Grey"/>
      </visual>
    </link>

<!-- gripper right finger -->

    <joint name="${prefix}_hande_right_finger_joint" type="prismatic">
      <origin rpy="0 0 0" xyz="0 0.025 0.0345"/>
      <parent link="${prefix}_hande_link"/>
      <child link="${prefix}_hande_right_finger"/>
      <axis xyz="0 -1 0"/>
      <limit effort="130" lower="0" upper="0.025" velocity="0.15"/>
      <mimic joint="${prefix}_hande_left_finger_joint" multiplier="1" offset="0"/>
    </joint>

    <link name="${prefix}_hande_right_finger">
      <inertial>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value="0.03804"/>
        <inertia
          ixx="1E-9" ixy="0.0" ixz="0.0" 
          iyy="1E-9" iyz="0.0" 
          izz="1E-9"
        />
      </inertial>

      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://robotiq_hande_description/meshes/finger_collision.dae" scale="1.0 1.0 1.0"/>
        </geometry>
      </collision>

      <visual>
        <origin rpy="1.570796 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://robotiq_hande_description/meshes/finger.dae" scale="1.0 1.0 1.0"/>
        </geometry>
        <material name="Grey" />
      </visual>
    </link>

<!--end of gripper frame -->

    <joint name="${prefix}_robotiq_hande_end_joint" type="fixed">
      <origin rpy="0 0 0" xyz="0 0 0.095"/>
      <parent link="${prefix}_hande_link"/>
      <child link="${prefix}_hande_end"/>
    </joint>

    <link name="${prefix}_hande_end" />

  </xacro:macro>

</robot>
fmauch commented 1 month ago

Closed in #1115