Program Listing for File conversions.hpp
↰ Return to documentation for file (/tmp/ws/src/octomap_ros/include/octomap_ros/conversions.hpp
)
// Copyright 2011, A. Hornung, University of Freiburg. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// * Neither the name of the Willow Garage nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#ifndef OCTOMAP_ROS__CONVERSIONS_HPP_
#define OCTOMAP_ROS__CONVERSIONS_HPP_
#include <octomap/octomap.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Vector3.h>
#include <geometry_msgs/msg/point.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
namespace octomap
{
void pointsOctomapToPointCloud2(
const octomap::point3d_list & points,
sensor_msgs::msg::PointCloud2 & cloud);
void pointCloud2ToOctomap(
const sensor_msgs::msg::PointCloud2 & cloud,
octomap::Pointcloud & octomapCloud);
static inline geometry_msgs::msg::Point pointOctomapToMsg(const octomap::point3d & octomapPt)
{
return geometry_msgs::build<geometry_msgs::msg::Point>()
.x(octomapPt.x())
.y(octomapPt.y())
.z(octomapPt.z());
}
static inline octomap::point3d pointMsgToOctomap(const geometry_msgs::msg::Point & ptMsg)
{
return octomap::point3d(ptMsg.x, ptMsg.y, ptMsg.z);
}
static inline tf2::Vector3 pointOctomapToTf(const point3d & octomapPt)
{
return tf2::Vector3(octomapPt.x(), octomapPt.y(), octomapPt.z());
}
static inline octomap::point3d pointTfToOctomap(const tf2::Vector3 & vTf)
{
return point3d(vTf.x(), vTf.y(), vTf.z());
}
static inline tf2::Quaternion quaternionOctomapToTf(const octomath::Quaternion & octomapQ)
{
return tf2::Quaternion(octomapQ.x(), octomapQ.y(), octomapQ.z(), octomapQ.u());
}
static inline octomath::Quaternion quaternionTfToOctomap(const tf2::Quaternion & qTf)
{
return octomath::Quaternion(qTf.w(), qTf.x(), qTf.y(), qTf.z());
}
static inline tf2::Transform poseOctomapToTf(const octomap::pose6d & octomapPose)
{
return tf2::Transform(
quaternionOctomapToTf(octomapPose.rot()),
pointOctomapToTf(octomapPose.trans()));
}
static inline octomap::pose6d poseTfToOctomap(const tf2::Transform & transformTf)
{
return octomap::pose6d(
pointTfToOctomap(transformTf.getOrigin()),
quaternionTfToOctomap(transformTf.getRotation()));
}
} // namespace octomap
#endif // OCTOMAP_ROS__CONVERSIONS_HPP_