pose_se2.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
39 #ifndef POSE_SE2_H_
40 #define POSE_SE2_H_
41 
42 #include <g2o/stuff/misc.h>
43 
44 #include <Eigen/Core>
45 #include <teb_local_planner/misc.h>
46 #include <geometry_msgs/Pose.h>
47 #include <tf/transform_datatypes.h>
48 
49 namespace teb_local_planner
50 {
51 
57 class PoseSE2
58 {
59 public:
60 
62 
68  {
69  setZero();
70  }
71 
77  PoseSE2(const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
78  {
80  _theta = theta;
81  }
82 
89  PoseSE2(double x, double y, double theta)
90  {
91  _position.coeffRef(0) = x;
92  _position.coeffRef(1) = y;
93  _theta = theta;
94  }
95 
100  PoseSE2(const geometry_msgs::Pose& pose)
101  {
102  _position.coeffRef(0) = pose.position.x;
103  _position.coeffRef(1) = pose.position.y;
104  _theta = tf::getYaw( pose.orientation );
105  }
106 
111  PoseSE2(const tf::Pose& pose)
112  {
113  _position.coeffRef(0) = pose.getOrigin().getX();
114  _position.coeffRef(1) = pose.getOrigin().getY();
115  _theta = tf::getYaw( pose.getRotation() );
116  }
117 
122  PoseSE2(const PoseSE2& pose)
123  {
124  _position = pose._position;
125  _theta = pose._theta;
126  }
127 
129 
130 
134  ~PoseSE2() {}
135 
136 
138 
145  Eigen::Vector2d& position() {return _position;}
146 
152  const Eigen::Vector2d& position() const {return _position;}
153 
158  double& x() {return _position.coeffRef(0);}
159 
164  const double& x() const {return _position.coeffRef(0);}
165 
170  double& y() {return _position.coeffRef(1);}
171 
176  const double& y() const {return _position.coeffRef(1);}
177 
182  double& theta() {return _theta;}
183 
188  const double& theta() const {return _theta;}
189 
193  void setZero()
194  {
195  _position.setZero();
196  _theta = 0;
197  }
198 
203  void toPoseMsg(geometry_msgs::Pose& pose) const
204  {
205  pose.position.x = _position.x();
206  pose.position.y = _position.y();
207  pose.position.z = 0;
208  pose.orientation = tf::createQuaternionMsgFromYaw(_theta);
209  }
210 
215  Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(std::cos(_theta), std::sin(_theta));}
216 
218 
219 
221 
227  void scale(double factor)
228  {
229  _position *= factor;
230  _theta = g2o::normalize_theta( _theta*factor );
231  }
232 
238  void plus(const double* pose_as_array)
239  {
240  _position.coeffRef(0) += pose_as_array[0];
241  _position.coeffRef(1) += pose_as_array[1];
242  _theta = g2o::normalize_theta( _theta + pose_as_array[2] );
243  }
244 
252  void averageInPlace(const PoseSE2& pose1, const PoseSE2& pose2)
253  {
254  _position = (pose1._position + pose2._position)/2;
255  _theta = g2o::average_angle(pose1._theta, pose2._theta);
256  }
257 
266  static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
267  {
268  return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
269  }
270 
279  void rotateGlobal(double angle, bool adjust_theta=true)
280  {
281  double new_x = std::cos(angle)*_position.x() - std::sin(angle)*_position.y();
282  double new_y = std::sin(angle)*_position.x() + std::cos(angle)*_position.y();
283  _position.x() = new_x;
284  _position.y() = new_y;
285  if (adjust_theta)
286  _theta = g2o::normalize_theta(_theta+angle);
287  }
288 
290 
291 
293 
300  PoseSE2& operator=( const PoseSE2& rhs )
301  {
302  if (&rhs != this)
303  {
304  _position = rhs._position;
305  _theta = rhs._theta;
306  }
307  return *this;
308  }
309 
315  {
316  _position += rhs._position;
317  _theta = g2o::normalize_theta(_theta + rhs._theta);
318  return *this;
319  }
320 
326  friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2& rhs)
327  {
328  return lhs += rhs;
329  }
330 
336  {
337  _position -= rhs._position;
338  _theta = g2o::normalize_theta(_theta - rhs._theta);
339  return *this;
340  }
341 
347  friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2& rhs)
348  {
349  return lhs -= rhs;
350  }
351 
359  friend PoseSE2 operator*(PoseSE2 pose, double scalar)
360  {
361  pose._position *= scalar;
362  pose._theta *= scalar;
363  return pose;
364  }
365 
373  friend PoseSE2 operator*(double scalar, PoseSE2 pose)
374  {
375  pose._position *= scalar;
376  pose._theta *= scalar;
377  return pose;
378  }
379 
385  friend std::ostream& operator<< (std::ostream& stream, const PoseSE2& pose)
386  {
387  stream << "x: " << pose._position[0] << " y: " << pose._position[1] << " theta: " << pose._theta;
388  return stream;
389  }
390 
392 
393 
394 private:
395 
396  Eigen::Vector2d _position;
397  double _theta;
398 
399 public:
400  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
401 };
402 
403 
404 } // namespace teb_local_planner
405 
406 #endif // POSE_SE2_H_
PoseSE2 & operator-=(const PoseSE2 &rhs)
Compound assignment operator (subtraction)
Definition: pose_se2.h:335
Eigen::Vector2d & position()
Access the 2D position part.
Definition: pose_se2.h:145
~PoseSE2()
Destructs the PoseSE2.
Definition: pose_se2.h:134
static double getYaw(const Quaternion &bt_q)
PoseSE2 & operator+=(const PoseSE2 &rhs)
Compound assignment operator (addition)
Definition: pose_se2.h:314
static PoseSE2 average(const PoseSE2 &pose1, const PoseSE2 &pose2)
Get the mean / average of two poses and return the result (static) For the position part: 0...
Definition: pose_se2.h:266
PoseSE2 & operator=(const PoseSE2 &rhs)
Asignment operator.
Definition: pose_se2.h:300
PoseSE2(const tf::Pose &pose)
Construct pose using a tf::Pose.
Definition: pose_se2.h:111
TFSIMD_FORCE_INLINE const tfScalar & getY() const
double & x()
Access the x-coordinate the pose.
Definition: pose_se2.h:158
void averageInPlace(const PoseSE2 &pose1, const PoseSE2 &pose2)
Get the mean / average of two poses and store it in the caller class For the position part: 0...
Definition: pose_se2.h:252
Eigen::Vector2d orientationUnitVec() const
Return the unit vector of the current orientation.
Definition: pose_se2.h:215
friend std::ostream & operator<<(std::ostream &stream, const PoseSE2 &pose)
Output stream operator.
Definition: pose_se2.h:385
const double & y() const
Access the y-coordinate the pose (read-only)
Definition: pose_se2.h:176
void scale(double factor)
Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi]. ...
Definition: pose_se2.h:227
const double & theta() const
Access the orientation part (yaw angle) of the pose (read-only)
Definition: pose_se2.h:188
PoseSE2()
Default constructor.
Definition: pose_se2.h:67
void toPoseMsg(geometry_msgs::Pose &pose) const
Convert PoseSE2 to a geometry_msgs::Pose.
Definition: pose_se2.h:203
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
double & y()
Access the y-coordinate the pose.
Definition: pose_se2.h:170
This class implements a pose in the domain SE2: The pose consist of the position x and y and an orie...
Definition: pose_se2.h:57
double & theta()
Access the orientation part (yaw angle) of the pose.
Definition: pose_se2.h:182
friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2 &rhs)
Arithmetic operator overload for subtractions.
Definition: pose_se2.h:347
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
Quaternion getRotation() const
TFSIMD_FORCE_INLINE const tfScalar & getX() const
const Eigen::Vector2d & position() const
Access the 2D position part (read-only)
Definition: pose_se2.h:152
PoseSE2(double x, double y, double theta)
Construct pose using single components x, y, and the yaw angle.
Definition: pose_se2.h:89
void rotateGlobal(double angle, bool adjust_theta=true)
Rotate pose globally.
Definition: pose_se2.h:279
friend PoseSE2 operator*(double scalar, PoseSE2 pose)
Multiply pose with scalar and return copy without normalizing theta This operator is useful for calcu...
Definition: pose_se2.h:373
void plus(const double *pose_as_array)
Increment the pose by adding a double[3] array The angle is normalized afterwards.
Definition: pose_se2.h:238
friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2 &rhs)
Arithmetic operator overload for additions.
Definition: pose_se2.h:326
const double & x() const
Access the x-coordinate the pose (read-only)
Definition: pose_se2.h:164
PoseSE2(const Eigen::Ref< const Eigen::Vector2d > &position, double theta)
Construct pose given a position vector and an angle theta.
Definition: pose_se2.h:77
PoseSE2(const geometry_msgs::Pose &pose)
Construct pose using a geometry_msgs::Pose.
Definition: pose_se2.h:100
friend PoseSE2 operator*(PoseSE2 pose, double scalar)
Multiply pose with scalar and return copy without normalizing theta This operator is useful for calcu...
Definition: pose_se2.h:359
PoseSE2(const PoseSE2 &pose)
Copy constructor.
Definition: pose_se2.h:122
Eigen::Vector2d _position
Definition: pose_se2.h:396
void setZero()
Set pose to [0,0,0].
Definition: pose_se2.h:193


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10