tf_filter.h

Go to the documentation of this file.

/****************************************************************************
 *
 * fkie_message_filters
 * Copyright © 2018-2020 Fraunhofer FKIE
 * Author: Timo Röhling
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 ****************************************************************************/

#ifndef INCLUDE_FKIE_MESSAGE_FILTERS_TF_FILTER_H_
#define INCLUDE_FKIE_MESSAGE_FILTERS_TF_FILTER_H_

#include "filter.h"
#include <tf2/buffer_core.h>
#include <ros/node_handle.h>
#include <ros/callback_queue_interface.h>

namespace fkie_message_filters
{

enum class TfFilterResult
{
    TransformAvailable,
    EmptyFrameID,
    TransformExpired,
    TransformFailed,
    QueueOverflow,
    UnknownFailure
};

template<class... Inputs>
class TfFilter : public Filter<IO<Inputs...>, IO<Inputs...>>
{
public:
    using FilterFailureCB = std::function<void(const Inputs&..., TfFilterResult)>;

    TfFilter() noexcept {}
    TfFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh) noexcept;
    TfFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbq) noexcept;
    TfFilter(tf2::BufferCore& bc, const ros::V_string& target_frames, uint32_t queue_size, const ros::NodeHandle& nh) noexcept;
    TfFilter(tf2::BufferCore& bc, const ros::V_string& target_frames, uint32_t queue_size, ros::CallbackQueueInterface* cbq) noexcept;
    void init(tf2::BufferCore& bc, uint32_t queue_size, const ros::NodeHandle& nh) noexcept;
    void init(tf2::BufferCore& bc, uint32_t queue_size, ros::CallbackQueueInterface* cbq) noexcept;
    void set_target_frame (const std::string& target_frame);
    void set_target_frames (const ros::V_string& target_frames);
    void reset() noexcept override;
    void set_filter_failure_function(FilterFailureCB cb);
protected:
    void receive (const Inputs&... in) override;
private:
    using MessageTuple = std::tuple<Inputs...>;
    struct Impl;
    class RosCB;
    std::shared_ptr<Impl> impl_;
    void transformable(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame, ros::Time time, tf2::TransformableResult result);
    void report_failure (std::unique_lock<std::mutex>&, const MessageTuple&, TfFilterResult);
    void send_message (std::unique_lock<std::mutex>&, const MessageTuple& m);
};

template<class... Inputs>
class TfFilter<IO<Inputs...>> : public TfFilter<Inputs...> {};

} // namespace fkie_message_filters

#include "tf_filter_impl.h"

#endif /* INCLUDE_FKIE_MESSAGE_FILTERS_TF_FILTER_H_ */