tf2_autoware_auto_msgs.hpp

Go to the documentation of this file.

// Copyright 2020, The Autoware Foundation.
//
// 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 AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_
#define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_

#include <tf2/convert.h>
#include <tf2/time.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <autoware_auto_msgs/msg/bounding_box_array.hpp>
#include <autoware_auto_msgs/msg/bounding_box.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <autoware_auto_msgs/msg/quaternion32.hpp>
#include <geometry_msgs/msg/point32.hpp>
#include <kdl/frames.hpp>
#include <common/types.hpp>
#include <string>


using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
using BoundingBoxArray = autoware_auto_msgs::msg::BoundingBoxArray;
using BoundingBox = autoware_auto_msgs::msg::BoundingBox;

namespace tf2
{


/*************/
/*************/

template<>
inline
void doTransform(
  const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
  t_out.x = static_cast<float>(v_out[0]);
  t_out.y = static_cast<float>(v_out[1]);
  t_out.z = static_cast<float>(v_out[2]);
}

/******************/
/******************/

template<>
inline
void doTransform(
  const autoware_auto_msgs::msg::Quaternion32 & t_in,
  autoware_auto_msgs::msg::Quaternion32 & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  KDL::Rotation r_in = KDL::Rotation::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w);
  KDL::Rotation out = gmTransformToKDL(transform).M * r_in;

  double qx, qy, qz, qw;
  out.GetQuaternion(qx, qy, qz, qw);
  t_out.x = static_cast<float32_t>(qx);
  t_out.y = static_cast<float32_t>(qy);
  t_out.z = static_cast<float32_t>(qz);
  t_out.w = static_cast<float32_t>(qw);
}


/******************/
/******************/

template<>
inline
void doTransform(
  const BoundingBox & t_in, BoundingBox & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = t_in;
  doTransform(t_in.orientation, t_out.orientation, transform);
  doTransform(t_in.centroid, t_out.centroid, transform);
  doTransform(t_in.corners[0], t_out.corners[0], transform);
  doTransform(t_in.corners[1], t_out.corners[1], transform);
  doTransform(t_in.corners[2], t_out.corners[2], transform);
  doTransform(t_in.corners[3], t_out.corners[3], transform);
  // TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size
}


/**********************/
/**********************/

template<>
inline
tf2::TimePoint getTimestamp(const BoundingBoxArray & t)
{
  return tf2_ros::fromMsg(t.header.stamp);
}

template<>
inline
std::string getFrameId(const BoundingBoxArray & t) {return t.header.frame_id;}

template<>
inline
void doTransform(
  const BoundingBoxArray & t_in,
  BoundingBoxArray & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = t_in;
  for (auto idx = 0U; idx < t_in.boxes.size(); idx++) {
    doTransform(t_out.boxes[idx], t_out.boxes[idx], transform);
  }
  t_out.header.stamp = transform.header.stamp;
  t_out.header.frame_id = transform.header.frame_id;
}

}  // namespace tf2

#endif  // AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_