The ParsePointCloud2 TemplateΒΆ

ParsePointCloud2

smacha/src/smacha_ros/templates/ParsePointCloud2.tpl.py

{% block meta %}
name: ParsePointCloud2
description:
  SMACH template that provides a ParsePointCloud helper function for, e.g.
  PublishMsgState. The function takes an input argument that can be specified
  as either a sensor_msgs.msg.PointCloud or sensor_msgs.msg.PointCloud2 type,
  and returns a sensor_msgs.msg.PointCloud2 type.
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends: []
variables: []
input_keys: []
output_keys: []
{% endblock meta %}

{% from "Utils.tpl.py" import from_import %}

{% block imports %}
{{ from_import(defined_headers, 'sensor_msgs.msg', 'PointCloud') }}
{{ from_import(defined_headers, 'sensor_msgs.msg', 'PointCloud2') }}
{{ from_import(defined_headers, 'sensor_msgs.point_cloud2', 'create_cloud_xyz32') }}
{% endblock imports %}

{% block defs %}
{% if 'def_parse_pointcloud2' not in defined_headers %}
def parse_pointcloud2(pointcloud_input):
    """
    Parse pointcloud_input into PointCloud2.
    """
    try:
        assert isinstance(pointcloud_input, PointCloud2)
        return pointcloud_input
    except:
        pass
    try:
        points = [[point.x, point.y, point.z] for point in pointcloud_input.points]
        pointcloud2 = create_cloud_xyz32(header=pointcloud_input.header, points=points)
        return pointcloud2
    except:
        raise ValueError('Point cloud not properly specified (should be PointCloud or PointCloud2 type)!')
{% do defined_headers.append('def_parse_pointcloud2') %}{% endif %}
{% endblock defs %}