Gazebo Msgs

API Reference

10.1.1
Pose.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16 */
17#ifndef GZ_MSGS_CONVERT_POSE_HH_
18#define GZ_MSGS_CONVERT_POSE_HH_
19
22
23// Message Headers
24#include "gz/msgs/quaternion.pb.h"
25#include "gz/msgs/pose.pb.h"
26
27// Data Headers
28#include <gz/math/Pose3.hh>
29
30namespace gz::msgs {
31// Inline bracket to help doxygen filtering.
32inline namespace GZ_MSGS_VERSION_NAMESPACE {
33
35inline void Set(gz::msgs::Pose *_msg, const gz::math::Pose3d &_data)
36{
37 Set(_msg->mutable_position(), _data.Pos());
38 Set(_msg->mutable_orientation(), _data.Rot());
39}
40
41inline void Set(gz::math::Pose3d *_data, const gz::msgs::Pose &_msg)
42{
44 gz::math::Quaterniond orientation;
45
46 if (_msg.has_position())
47 pos = Convert(_msg.position());
48
49 // This bit is critical. If orientation hasn't been set in the message,
50 // then we want the quaternion to default to identity.
51 if (_msg.has_orientation())
52 orientation = Convert(_msg.orientation());
53
54 _data->Set(pos, orientation);
55}
56
57inline gz::msgs::Pose Convert(const gz::math::Pose3d &_data)
58{
59 gz::msgs::Pose ret;
60 Set(&ret, _data);
61 return ret;
62}
63
64inline gz::math::Pose3d Convert(const gz::msgs::Pose &_msg)
65{
67 Set(&ret, _msg);
68 return ret;
69}
70} // namespce
71} // namespace gz::msgs
72
73#endif // GZ_MSGS_CONVERT_VECTOR3_HH_