Program Listing for File geometry_msgs.hpp

↰ Return to documentation for file (/tmp/ws/src/gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp)

// Copyright 2018 Open Source Robotics Foundation, Inc.
//
// 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 GAZEBO_ROS__CONVERSIONS__GEOMETRY_MSGS_HPP_
#define GAZEBO_ROS__CONVERSIONS__GEOMETRY_MSGS_HPP_

#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/point32.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Quaternion.hh>
#include <ignition/math/Vector3.hh>

#include "gazebo_ros/conversions/generic.hpp"

namespace gazebo_ros
{

template<class T>
inline
T Convert(const geometry_msgs::msg::Vector3 &)
{
  T::ConversionNotImplemented;
}

template<>
inline
ignition::math::Vector3d Convert(const geometry_msgs::msg::Vector3 & msg)
{
  ignition::math::Vector3d vec;
  vec.X(msg.x);
  vec.Y(msg.y);
  vec.Z(msg.z);
  return vec;
}

template<class T>
inline
T Convert(const geometry_msgs::msg::Point32 &)
{
  T::ConversionNotImplemented;
}

template<>
inline
ignition::math::Vector3d Convert(const geometry_msgs::msg::Point32 & in)
{
  ignition::math::Vector3d vec;
  vec.X(in.x);
  vec.Y(in.y);
  vec.Z(in.z);
  return vec;
}

template<class T>
inline
T Convert(const geometry_msgs::msg::Point &)
{
  T::ConversionNotImplemented;
}

template<>
inline
geometry_msgs::msg::Vector3 Convert(const geometry_msgs::msg::Point & in)
{
  geometry_msgs::msg::Vector3 msg;
  msg.x = in.x;
  msg.y = in.y;
  msg.z = in.z;
  return msg;
}

template<>
inline
ignition::math::Vector3d Convert(const geometry_msgs::msg::Point & in)
{
  ignition::math::Vector3d out;
  out.X(in.x);
  out.Y(in.y);
  out.Z(in.z);
  return out;
}

template<>
inline
geometry_msgs::msg::Vector3 Convert(const ignition::math::Vector3d & vec)
{
  geometry_msgs::msg::Vector3 msg;
  msg.x = vec.X();
  msg.y = vec.Y();
  msg.z = vec.Z();
  return msg;
}

template<>
inline
geometry_msgs::msg::Point Convert(const ignition::math::Vector3d & vec)
{
  geometry_msgs::msg::Point msg;
  msg.x = vec.X();
  msg.y = vec.Y();
  msg.z = vec.Z();
  return msg;
}

template<>
inline
geometry_msgs::msg::Quaternion Convert(const ignition::math::Quaterniond & in)
{
  geometry_msgs::msg::Quaternion msg;
  msg.x = in.X();
  msg.y = in.Y();
  msg.z = in.Z();
  msg.w = in.W();
  return msg;
}

template<>
inline
geometry_msgs::msg::Transform Convert(const ignition::math::Pose3d & in)
{
  geometry_msgs::msg::Transform msg;
  msg.translation = Convert<geometry_msgs::msg::Vector3>(in.Pos());
  msg.rotation = Convert<geometry_msgs::msg::Quaternion>(in.Rot());
  return msg;
}

template<>
inline
geometry_msgs::msg::Pose Convert(const ignition::math::Pose3d & in)
{
  geometry_msgs::msg::Pose msg;
  msg.position = Convert<geometry_msgs::msg::Point>(in.Pos());
  msg.orientation = Convert<geometry_msgs::msg::Quaternion>(in.Rot());
  return msg;
}

template<class T>
inline
T Convert(const geometry_msgs::msg::Quaternion &)
{
  T::ConversionNotImplemented;
}

template<>
inline
ignition::math::Quaterniond Convert(const geometry_msgs::msg::Quaternion & in)
{
  return ignition::math::Quaterniond(in.w, in.x, in.y, in.z);
}

template<class T>
inline
T Convert(const geometry_msgs::msg::Transform &)
{
  T::ConversionNotImplemented;
}

template<>
inline
ignition::math::Pose3d Convert(const geometry_msgs::msg::Transform & in)
{
  ignition::math::Pose3d msg;
  msg.Pos() = Convert<ignition::math::Vector3d>(in.translation);
  msg.Rot() = Convert<ignition::math::Quaterniond>(in.rotation);
  return msg;
}

template<class T>
inline
T Convert(const geometry_msgs::msg::Pose &)
{
  T::ConversionNotImplemented;
}

template<>
inline
geometry_msgs::msg::Transform Convert(const geometry_msgs::msg::Pose & in)
{
  geometry_msgs::msg::Transform msg;
  msg.translation = Convert<geometry_msgs::msg::Vector3>(in.position);
  msg.rotation = in.orientation;
  return msg;
}

template<>
inline
ignition::math::Pose3d Convert(const geometry_msgs::msg::Pose & in)
{
  return {Convert<ignition::math::Vector3d>(in.position),
    Convert<ignition::math::Quaterniond>(in.orientation)};
}

}  // namespace gazebo_ros
#endif  // GAZEBO_ROS__CONVERSIONS__GEOMETRY_MSGS_HPP_