transforms.orogen

# frozen_string_literal: true

name "transforms"
version "0.1"

import_types_from "transformsTypes.hpp"

import_types_from "std"
import_types_from "base"

# Task that changes the source frame of a pose and twist combination expressed
# as RigidBodyState
#
# This task changes the source frame of a RBS, keeping the target frame the same.
# It can for instance provide a body2world transform from a imu2world transform
# and a body2imu generated by the transformer
#
# The reference frame is immaterial to these transformations and therefore does not
# need to be provided
task_context "PoseAndTwistFrameChangeRBSTask" do
    needs_configuration

    # The pose+velocity of the source frame w.r.t. the reference frame
    input_port "source2ref_samples", "/base/samples/RigidBodyState"

    # The pose+velocity of the target frame w.r.t. the reference frame
    output_port "target2ref_samples", "/base/samples/RigidBodyState"

    transformer do
        transform "source", "target"
        align_port "source2ref_samples"
        # Note: will have to be configured in the component's configuration file
        max_latency 1
    end

    port_driven
end

# Task that changes the reference frame of a pose expressed as RigidBodyState
#
# This task changes the reference frame of a RBS, keeping the source frame the same.
task_context "PoseReferenceChangeRBSTask" do
    needs_configuration

    # The pose of the source frame w.r.t. the reference frame
    input_port "source2ref_samples", "/base/samples/RigidBodyState"

    # The pose of the source frame w.r.t. the new reference frame
    output_port "source2new_ref_samples", "/base/samples/RigidBodyState"

    transformer do
        transform "new_ref", "ref"
        align_port "source2ref_samples"
        # Note: will have to be configured in the component's configuration file
        max_latency 1
    end

    port_driven
end

# Task that takes two poses relative to a single reference and outputs the one
# relative to the other
#
# 'newref' is considered fixed in 'ref' for this calculation. That is, the
# linear velocity is rotated to match b's orientation, but the velocities are
# not combined
task_context "PoseCombinationRBSTask" do
    needs_configuration

    input_port "object2ref_pose", "/base/samples/RigidBodyState"
    input_port "newref2ref_pose", "/base/samples/RigidBodyState"

    output_port "object2newref_pose", "/base/samples/RigidBodyState"
    port_driven
end