route_point_inline.h
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2016, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 #ifndef SWRI_ROUTE_UTIL_ROUTE_POINT_INLINE_H_
30 #define SWRI_ROUTE_UTIL_ROUTE_POINT_INLINE_H_
31 
32 #include <boost/lexical_cast.hpp>
33 
34 namespace swri_route_util
35 {
36 inline
37 void RoutePoint::setPosition(const tf::Vector3 &position)
38 {
40 }
41 
42 inline
43 const tf::Vector3& RoutePoint::position() const
44 {
45  return position_;
46 }
47 
48 inline
49 tf::Vector3& RoutePoint::position()
50 {
51  return position_;
52 }
53 
54 inline
55 void RoutePoint::setPosition(const geometry_msgs::Point &position)
56 {
57  tf::pointMsgToTF(position, position_);
58 }
59 
60 inline
61 const geometry_msgs::Point RoutePoint::positionMsg() const
62 {
63  geometry_msgs::Point p;
65  return p;
66 }
67 
68 inline
70 {
72 }
73 
74 inline
76 {
77  return orientation_;
78 }
79 
80 inline
82 {
83  return orientation_;
84 }
85 
86 inline
87 void RoutePoint::setOrientation(const geometry_msgs::Quaternion &orientation)
88 {
89  tf::quaternionMsgToTF(orientation, orientation_);
90 }
91 
92 inline
93 const geometry_msgs::Quaternion RoutePoint::orientationMsg() const
94 {
95  geometry_msgs::Quaternion q;
97  return q;
98 }
99 
100 inline
102 {
103  setPosition(pose.getOrigin());
104  setOrientation(pose.getRotation());
105 }
106 
107 inline
109 {
110  tf::Pose pose;
111  pose.setOrigin(position_);
113  return pose;
114 }
115 
116 inline
117 void RoutePoint::setPose(const geometry_msgs::Pose &pose)
118 {
119  setPosition(pose.position);
120  setOrientation(pose.orientation);
121 }
122 
123 inline
124 geometry_msgs::Pose RoutePoint::poseMsg() const
125 {
126  geometry_msgs::Pose pose;
127  pose.position = positionMsg();
128  pose.orientation = orientationMsg();
129  return pose;
130 }
131 
132 inline
133 const std::string& RoutePoint::id() const
134 {
135  return id_;
136 }
137 
138 inline
139 void RoutePoint::setId(const std::string &id)
140 {
141  id_ = id;
142 }
143 
144 inline
145 marti_nav_msgs::RoutePosition RoutePoint::routePosition() const
146 {
147  marti_nav_msgs::RoutePosition position;
148  position.id = id();
149  position.distance = 0.0;
150  return position;
151 }
152 
153 inline
155 {
156  return stop_point_;
157 }
158 
159 inline
160 void RoutePoint::setStopPoint(bool value)
161 {
162  stop_point_ = value;
163 }
164 
165 inline
167 {
168  return stop_point_delay_;
169 }
170 
171 inline
173 {
174  stop_point_delay_ = delay;
175 }
176 
177 template <typename T>
178 inline
179 T RoutePoint::getTypedProperty(const std::string &name) const
180 {
181  return boost::lexical_cast<T>(getProperty(name));
182 }
183 } // namespace swri_route_util
184 #endif // SWRI_ROUTE_UTIL_ROUTE_POINT_INLINE_H_
void setPose(const tf::Pose &pose)
void setStopPointDelay(double delay)
const std::string & id() const
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
geometry_msgs::Pose poseMsg() const
Quaternion getRotation() const
const tf::Vector3 & position() const
T getTypedProperty(const std::string &name) const
marti_nav_msgs::RoutePosition routePosition() const
void setPosition(const tf::Vector3 &position)
tf::Quaternion orientation_
Definition: route_point.h:135
void setId(const std::string &id)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
std::string getProperty(const std::string &name) const
Definition: route_point.cpp:52
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
const geometry_msgs::Quaternion orientationMsg() const
void setOrientation(const tf::Quaternion &orientation)
const geometry_msgs::Point positionMsg() const
const tf::Quaternion & orientation() const
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
static void pointTFToMsg(const Point &bt_v, geometry_msgs::Point &msg_v)


swri_route_util
Author(s):
autogenerated on Sat Jan 21 2023 03:13:29