image_transport_codecs.h

Go to the documentation of this file.

#pragma once

// SPDX-License-Identifier: BSD-3-Clause
// SPDX-FileCopyrightText: Czech Technical University in Prague

#include <list>
#include <memory>
#include <string>
#include <unordered_map>

#include <dynamic_reconfigure/Config.h>
#include <pluginlib/class_loader.hpp>
#include <topic_tools/shape_shifter.h>

#include <cras_cpp_common/c_api.h>
#include <cras_cpp_common/log_utils.h>
#include <cras_cpp_common/log_utils/node.h>
#include <cras_cpp_common/xmlrpc_value_utils.hpp>
#include <cras_topic_tools/shape_shifter.h>
#include <image_transport_codecs/image_transport_codec.h>
#include <image_transport_codecs/image_transport_codec_plugin.h>

namespace image_transport_codecs
{

class ImageTransportCodecs : public cras::HasLogger
{
public:
  explicit ImageTransportCodecs(const cras::LogHelperPtr& log = std::make_shared<cras::NodeLogHelper>());

  virtual ~ImageTransportCodecs();

  void addCodec(const ImageTransportCodecPlugin::ConstPtr& codec);

  virtual ImageTransportCodec::EncodeResult encode(const sensor_msgs::Image& raw,
    const std::string& topicOrCodec, const dynamic_reconfigure::Config& config);

  virtual ImageTransportCodec::DecodeResult decode(const topic_tools::ShapeShifter& compressed,
    const std::string& topicOrCodec, const dynamic_reconfigure::Config& config);

  virtual ImageTransportCodec::GetCompressedContentResult getCompressedImageContent(
    const topic_tools::ShapeShifter& compressed, const std::string& topicOrCodec, const std::string& matchFormat) const;

  ImageTransportCodec::GetCompressedContentResult getCompressedImageContent(
    const topic_tools::ShapeShifter& compressed, const std::string& topicOrCodec) const;

  ImageTransportCodec::EncodeResult encode(const sensor_msgs::Image& raw, const std::string& topicOrCodec);

  ImageTransportCodec::DecodeResult decode(const topic_tools::ShapeShifter& compressed,
                                           const std::string& topicOrCodec);

  ImageTransportCodec::EncodeResult encode(const sensor_msgs::Image& raw,
                                           const std::string& topicOrCodec, const XmlRpc::XmlRpcValue& config);

  ImageTransportCodec::DecodeResult decode(const topic_tools::ShapeShifter& compressed,
                                           const std::string& topicOrCodec, const XmlRpc::XmlRpcValue& config);

  ImageTransportCodec::EncodeResult encode(const sensor_msgs::Image& raw,
    const std::string& topicOrCodec, const ros::NodeHandle& nh, const std::string& param);

  ImageTransportCodec::DecodeResult decode(const topic_tools::ShapeShifter& compressed,
    const std::string& topicOrCodec, const ros::NodeHandle& nh, const std::string& param);

  template<typename Config>
  ImageTransportCodec::EncodeResult encode(const sensor_msgs::Image& raw,
    const std::string& topicOrCodec, const Config& config)
  {
    dynamic_reconfigure::Config configMsg;
    config.__toMessage__(configMsg);
    return this->encode(raw, topicOrCodec, configMsg);
  }

  template<typename Config>
  ImageTransportCodec::DecodeResult decode(const topic_tools::ShapeShifter& compressed,
                                           const std::string& topicOrCodec, const Config& config)
  {
    dynamic_reconfigure::Config configMsg;
    config.__toMessage__(configMsg);
    return this->decode(compressed, topicOrCodec, configMsg);
  }

  template<typename M>
  inline cras::expected<M, std::string> encodeTyped(const sensor_msgs::Image& raw, const std::string& topicOrCodec,
                                                    const dynamic_reconfigure::Config& config)
  {
    const auto shifter = this->encode(raw, topicOrCodec, config);
    if (!shifter)
      return cras::make_unexpected(shifter.error());

    try
    {
      return *shifter->instantiate<M>();
    }
    catch (const ros::Exception& e)
    {
      return cras::make_unexpected(cras::format("Invalid shapeshifter returned from encoder: %s.", e.what()));
    }
  }

  template<typename M>
  inline ImageTransportCodec::DecodeResult decodeTyped(const M& compressed, const std::string& topicOrCodec,
                                                       const dynamic_reconfigure::Config& config)
  {
    topic_tools::ShapeShifter shifter;
    cras::msgToShapeShifter(compressed, shifter);
    return this->decode(shifter, topicOrCodec, config);
  }

  template<typename M>
  inline cras::expected<M, std::string> encodeTyped(const sensor_msgs::Image& raw, const std::string& topicOrCodec)
  {
    return this->encodeTyped<M>(raw, topicOrCodec, dynamic_reconfigure::Config());
  }

  template<typename M>
  inline ImageTransportCodec::DecodeResult decodeTyped(const M& compressed, const std::string& topicOrCodec)
  {
    return this->decodeTyped(compressed, topicOrCodec, dynamic_reconfigure::Config());
  }

  template<typename M>
  inline cras::expected<M, std::string> encodeTyped(const sensor_msgs::Image& raw, const std::string& topicOrCodec,
                                                    const XmlRpc::XmlRpcValue& config)
  {
    dynamic_reconfigure::Config configMsg;
    std::list<std::string> errors;
    if (!cras::convert(config, configMsg, true, &errors))
      return cras::make_unexpected("Invalid encoder config: " + cras::join(errors, " "));

    return this->encodeTyped<M>(raw, topicOrCodec, configMsg);
  }

  template<typename M>
  inline ImageTransportCodec::DecodeResult decodeTyped(const M& compressed, const std::string& topicOrCodec,
                                                       const XmlRpc::XmlRpcValue& config)
  {
    dynamic_reconfigure::Config configMsg;
    std::list<std::string> errors;
    if (!cras::convert(config, configMsg, true, &errors))
      return cras::make_unexpected("Invalid decoder config: " + cras::join(errors, " "));

    return this->decodeTyped(compressed, topicOrCodec, configMsg);
  }

  template<typename M>
  inline cras::expected<M, std::string> encodeTyped(const sensor_msgs::Image& raw, const std::string& topicOrCodec,
                                                    const ros::NodeHandle& nh, const std::string& param)
  {
    return this->encodeTyped<M>(raw, topicOrCodec, nh.param(param, XmlRpc::XmlRpcValue()));
  }

  template<typename M>
  inline ImageTransportCodec::DecodeResult decodeTyped(const M& compressed, const std::string& topicOrCodec,
                                                       const ros::NodeHandle& nh, const std::string& param)
  {
    return this->encodeTyped(compressed, topicOrCodec, nh.param(param, XmlRpc::XmlRpcValue()));
  }

  template<typename M, typename Config>
  inline cras::expected<M, std::string> encodeTyped(const sensor_msgs::Image& raw, const std::string& topicOrCodec,
                                                    const Config& config)
  {
    dynamic_reconfigure::Config configMsg;
    config.__toMessage__(configMsg);
    return this->encodeTyped<M>(raw, topicOrCodec, configMsg);
  }

  template<typename M, typename Config>
  inline ImageTransportCodec::DecodeResult decodeTyped(const M& compressed, const std::string& topicOrCodec,
                                                       const Config& config)
  {
    dynamic_reconfigure::Config configMsg;
    config.__toMessage__(configMsg);
    return this->decodeTyped(compressed, topicOrCodec, configMsg);
  }

protected:
  void loadCodecs();

  std::string parseTransport(const std::string& topicOrCodec) const;

  std::unique_ptr<pluginlib::ClassLoader<ImageTransportCodecPlugin>> loader;  
  std::unordered_map<std::string, ImageTransportCodecPlugin::ConstPtr> codecs;  
};

}


extern "C" bool imageTransportCodecsEncode(
  const char* topicOrCodec,
  sensor_msgs::Image::_height_type rawHeight,
  sensor_msgs::Image::_width_type rawWidth,
  const char* rawEncoding,
  sensor_msgs::Image::_is_bigendian_type rawIsBigEndian,
  sensor_msgs::Image::_step_type rawStep,
  size_t rawDataLength,
  const uint8_t rawData[],
  cras::allocator_t compressedTypeAllocator,
  cras::allocator_t md5SumAllocator,
  cras::allocator_t compressedDataAllocator,
  size_t serializedConfigLength,
  const uint8_t serializedConfig[],
  cras::allocator_t errorStringAllocator,
  cras::allocator_t logMessagesAllocator
);

extern "C" bool imageTransportCodecsDecode(
  const char* topicOrCodec,
  const char* compressedType,
  const char* compressedMd5sum,
  size_t compressedDataLength,
  const uint8_t compressedData[],
  sensor_msgs::Image::_height_type& rawHeight,
  sensor_msgs::Image::_width_type& rawWidth,
  cras::allocator_t rawEncodingAllocator,
  sensor_msgs::Image::_is_bigendian_type& rawIsBigEndian,
  sensor_msgs::Image::_step_type& rawStep,
  cras::allocator_t rawDataAllocator,
  size_t serializedConfigLength,
  const uint8_t serializedConfig[],
  cras::allocator_t errorStringAllocator,
  cras::allocator_t logMessagesAllocator
);

extern "C" bool getCompressedImageContents(
  const char* topicOrCodec,
  const char* compressedType,
  const char* compressedMd5sum,
  size_t compressedDataLength,
  const uint8_t compressedData[],
  const char* matchFormat,
  bool& hasData,
  cras::allocator_t formatAllocator,
  cras::allocator_t dataAllocator,
  cras::allocator_t errorStringAllocator,
  cras::allocator_t logMessagesAllocator
);