first commit
This commit is contained in:
152
include/pose_se2/misc.h
Normal file
152
include/pose_se2/misc.h
Normal file
@@ -0,0 +1,152 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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 OWNER 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.
|
||||
*
|
||||
* Author: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef MISC_H
|
||||
#define MISC_H
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <boost/utility.hpp>
|
||||
#include <boost/type_traits.hpp>
|
||||
|
||||
|
||||
namespace pose_se2
|
||||
{
|
||||
|
||||
#define SMALL_NUM 0.00000001
|
||||
|
||||
//! Symbols for left/none/right rotations
|
||||
enum class RotType { left, none, right };
|
||||
|
||||
/**
|
||||
* @brief Check whether two variables (double) are close to each other
|
||||
* @param a the first value to compare
|
||||
* @param b the second value to compare
|
||||
* @param epsilon precision threshold
|
||||
* @return \c true if |a-b| < epsilon, false otherwise
|
||||
*/
|
||||
inline bool is_close(double a, double b, double epsilon = 1e-4)
|
||||
{
|
||||
return std::fabs(a - b) < epsilon;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Return the average angle of an arbitrary number of given angles [rad]
|
||||
* @param angles vector containing all angles
|
||||
* @return average / mean angle, that is normalized to [-pi, pi]
|
||||
*/
|
||||
inline double average_angles(const std::vector<double>& angles)
|
||||
{
|
||||
double x=0, y=0;
|
||||
for (std::vector<double>::const_iterator it = angles.begin(); it!=angles.end(); ++it)
|
||||
{
|
||||
x += cos(*it);
|
||||
y += sin(*it);
|
||||
}
|
||||
if(x == 0 && y == 0)
|
||||
return 0;
|
||||
else
|
||||
return std::atan2(y, x);
|
||||
}
|
||||
|
||||
/** @brief Small helper function: check if |a|<|b| */
|
||||
inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(j);}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculate a fast approximation of a sigmoid function
|
||||
* @details The following function is implemented: \f$ x / (1 + |x|) \f$
|
||||
* @param x the argument of the function
|
||||
*/
|
||||
inline double fast_sigmoid(double x)
|
||||
{
|
||||
return x / (1 + fabs(x));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Calculate Euclidean distance between two 2D point datatypes
|
||||
* @param point1 object containing fields x and y
|
||||
* @param point2 object containing fields x and y
|
||||
* @return Euclidean distance: ||point2-point1||
|
||||
*/
|
||||
template <typename P1, typename P2>
|
||||
inline double distance_points2d(const P1& point1, const P2& point2)
|
||||
{
|
||||
return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) );
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d)
|
||||
* @param v1 object containing public methods x() and y()
|
||||
* @param v2 object containing fields x() and y()
|
||||
* @return magnitude that would result in the 3D case (along the z-axis)
|
||||
*/
|
||||
template <typename V1, typename V2>
|
||||
inline double cross2d(const V1& v1, const V2& v2)
|
||||
{
|
||||
return v1.x()*v2.y() - v2.x()*v1.y();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
|
||||
*
|
||||
* Return a constant reference for boths input variants (pointer or reference).
|
||||
* @remarks Makes only sense in combination with the overload getConstReference(const T& val).
|
||||
* @param ptr pointer of type T
|
||||
* @tparam T arbitrary type
|
||||
* @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
|
||||
*/
|
||||
template<typename T>
|
||||
inline const T& get_const_reference(const T* ptr) {return *ptr;}
|
||||
|
||||
/**
|
||||
* @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
|
||||
*
|
||||
* Return a constant reference for boths input variants (pointer or reference).
|
||||
* @remarks Makes only sense in combination with the overload getConstReference(const T* val).
|
||||
* @param val
|
||||
* @param dummy SFINAE helper variable
|
||||
* @tparam T arbitrary type
|
||||
* @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
|
||||
*/
|
||||
template<typename T>
|
||||
inline const T& get_const_reference(const T& val, typename boost::disable_if<boost::is_pointer<T> >::type* dummy = 0) {return val;}
|
||||
|
||||
} // namespace teb_local_planner
|
||||
|
||||
#endif /* MISC_H */
|
||||
395
include/pose_se2/pose_se2.h
Executable file
395
include/pose_se2/pose_se2.h
Executable file
@@ -0,0 +1,395 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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 OWNER 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.
|
||||
*
|
||||
* Author: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef ROBOT_POSE_SE2_H_
|
||||
#define ROBOT_POSE_SE2_H_
|
||||
|
||||
#include <g2o/stuff/misc.h>
|
||||
#include <Eigen/Core>
|
||||
#include <pose_se2/misc.h>
|
||||
#include <robot_geometry_msgs/Pose.h>
|
||||
#include <tf3/transform_datatypes.h>
|
||||
#include <data_convert/data_convert.h>
|
||||
|
||||
namespace pose_se2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class PoseSE2
|
||||
* @brief This class implements a pose in the domain SE2: \f$ \mathbb{R}^2 \times S^1 \f$
|
||||
* The pose consist of the position x and y and an orientation given as angle theta [-pi, pi].
|
||||
*/
|
||||
class PoseSE2
|
||||
{
|
||||
public:
|
||||
|
||||
/** @name Construct PoseSE2 instances */
|
||||
///@{
|
||||
|
||||
/**
|
||||
* @brief Default constructor
|
||||
*/
|
||||
PoseSE2()
|
||||
{
|
||||
setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct pose given a position vector and an angle theta
|
||||
* @param position 2D position vector
|
||||
* @param theta angle given in rad
|
||||
*/
|
||||
PoseSE2(const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
|
||||
{
|
||||
_position = position;
|
||||
_theta = theta;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct pose using single components x, y, and the yaw angle
|
||||
* @param x x-coordinate
|
||||
* @param y y-coordinate
|
||||
* @param theta yaw angle in rad
|
||||
*/
|
||||
PoseSE2(double x, double y, double theta)
|
||||
{
|
||||
_position.coeffRef(0) = x;
|
||||
_position.coeffRef(1) = y;
|
||||
_theta = theta;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct pose using a robot_geometry_msgs::Pose
|
||||
* @param pose robot_geometry_msgs::Pose object
|
||||
*/
|
||||
PoseSE2(const robot_geometry_msgs::Pose& pose)
|
||||
{
|
||||
_position.coeffRef(0) = pose.position.x;
|
||||
_position.coeffRef(1) = pose.position.y;
|
||||
_theta = data_convert::getYaw( pose.orientation );
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Copy constructor
|
||||
* @param pose PoseSE2 instance
|
||||
*/
|
||||
PoseSE2(const PoseSE2& pose)
|
||||
{
|
||||
_position = pose._position;
|
||||
_theta = pose._theta;
|
||||
}
|
||||
|
||||
///@}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Destructs the PoseSE2
|
||||
*/
|
||||
~PoseSE2() {}
|
||||
|
||||
|
||||
/** @name Access and modify values */
|
||||
///@{
|
||||
|
||||
/**
|
||||
* @brief Access the 2D position part
|
||||
* @see estimate
|
||||
* @return reference to the 2D position part
|
||||
*/
|
||||
Eigen::Vector2d& position() {return _position;}
|
||||
|
||||
/**
|
||||
* @brief Access the 2D position part (read-only)
|
||||
* @see estimate
|
||||
* @return const reference to the 2D position part
|
||||
*/
|
||||
const Eigen::Vector2d& position() const {return _position;}
|
||||
|
||||
/**
|
||||
* @brief Access the x-coordinate the pose
|
||||
* @return reference to the x-coordinate
|
||||
*/
|
||||
double& x() {return _position.coeffRef(0);}
|
||||
|
||||
/**
|
||||
* @brief Access the x-coordinate the pose (read-only)
|
||||
* @return const reference to the x-coordinate
|
||||
*/
|
||||
const double& x() const {return _position.coeffRef(0);}
|
||||
|
||||
/**
|
||||
* @brief Access the y-coordinate the pose
|
||||
* @return reference to the y-coordinate
|
||||
*/
|
||||
double& y() {return _position.coeffRef(1);}
|
||||
|
||||
/**
|
||||
* @brief Access the y-coordinate the pose (read-only)
|
||||
* @return const reference to the y-coordinate
|
||||
*/
|
||||
const double& y() const {return _position.coeffRef(1);}
|
||||
|
||||
/**
|
||||
* @brief Access the orientation part (yaw angle) of the pose
|
||||
* @return reference to the yaw angle
|
||||
*/
|
||||
double& theta() {return _theta;}
|
||||
|
||||
/**
|
||||
* @brief Access the orientation part (yaw angle) of the pose (read-only)
|
||||
* @return const reference to the yaw angle
|
||||
*/
|
||||
const double& theta() const {return _theta;}
|
||||
|
||||
/**
|
||||
* @brief Set pose to [0,0,0]
|
||||
*/
|
||||
void setZero()
|
||||
{
|
||||
_position.setZero();
|
||||
_theta = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert PoseSE2 to a robot_geometry_msgs::Pose
|
||||
* @param[out] pose Pose message
|
||||
*/
|
||||
void toPoseMsg(robot_geometry_msgs::Pose& pose) const
|
||||
{
|
||||
pose.position.x = _position.x();
|
||||
pose.position.y = _position.y();
|
||||
pose.position.z = 0;
|
||||
pose.orientation = data_convert::createQuaternionMsgFromYaw(_theta);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Return the unit vector of the current orientation
|
||||
* @returns [cos(theta), sin(theta))]^T
|
||||
*/
|
||||
Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(std::cos(_theta), std::sin(_theta));}
|
||||
|
||||
///@}
|
||||
|
||||
|
||||
/** @name Arithmetic operations for which operators are not always reasonable */
|
||||
///@{
|
||||
|
||||
/**
|
||||
* @brief Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi]
|
||||
* @param factor scale factor
|
||||
*/
|
||||
void scale(double factor)
|
||||
{
|
||||
_position *= factor;
|
||||
_theta = g2o::normalize_theta( _theta*factor );
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Increment the pose by adding a double[3] array
|
||||
* The angle is normalized afterwards
|
||||
* @param pose_as_array 3D double array [x, y, theta]
|
||||
*/
|
||||
void plus(const double* pose_as_array)
|
||||
{
|
||||
_position.coeffRef(0) += pose_as_array[0];
|
||||
_position.coeffRef(1) += pose_as_array[1];
|
||||
_theta = g2o::normalize_theta( _theta + pose_as_array[2] );
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the mean / average of two poses and store it in the caller class
|
||||
* For the position part: 0.5*(x1+x2)
|
||||
* For the angle: take the angle of the mean direction vector
|
||||
* @param pose1 first pose to consider
|
||||
* @param pose2 second pose to consider
|
||||
*/
|
||||
void averageInPlace(const PoseSE2& pose1, const PoseSE2& pose2)
|
||||
{
|
||||
_position = (pose1._position + pose2._position)/2;
|
||||
_theta = g2o::average_angle(pose1._theta, pose2._theta);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the mean / average of two poses and return the result (static)
|
||||
* For the position part: 0.5*(x1+x2)
|
||||
* For the angle: take the angle of the mean direction vector
|
||||
* @param pose1 first pose to consider
|
||||
* @param pose2 second pose to consider
|
||||
* @return mean / average of \c pose1 and \c pose2
|
||||
*/
|
||||
static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
|
||||
{
|
||||
return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Rotate pose globally
|
||||
*
|
||||
* Compute [pose_x, pose_y] = Rot(\c angle) * [pose_x, pose_y].
|
||||
* if \c adjust_theta, pose_theta is also rotated by \c angle
|
||||
* @param angle the angle defining the 2d rotation
|
||||
* @param adjust_theta if \c true, the orientation theta is also rotated
|
||||
*/
|
||||
void rotateGlobal(double angle, bool adjust_theta=true)
|
||||
{
|
||||
double new_x = std::cos(angle)*_position.x() - std::sin(angle)*_position.y();
|
||||
double new_y = std::sin(angle)*_position.x() + std::cos(angle)*_position.y();
|
||||
_position.x() = new_x;
|
||||
_position.y() = new_y;
|
||||
if (adjust_theta)
|
||||
_theta = g2o::normalize_theta(_theta+angle);
|
||||
}
|
||||
|
||||
///@}
|
||||
|
||||
|
||||
/** @name Operator overloads / Allow some arithmetic operations */
|
||||
///@{
|
||||
|
||||
/**
|
||||
* @brief Asignment operator
|
||||
* @param rhs PoseSE2 instance
|
||||
* @todo exception safe version of the assignment operator
|
||||
*/
|
||||
PoseSE2& operator=( const PoseSE2& rhs )
|
||||
{
|
||||
if (&rhs != this)
|
||||
{
|
||||
_position = rhs._position;
|
||||
_theta = rhs._theta;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compound assignment operator (addition)
|
||||
* @param rhs addend
|
||||
*/
|
||||
PoseSE2& operator+=(const PoseSE2& rhs)
|
||||
{
|
||||
_position += rhs._position;
|
||||
_theta = g2o::normalize_theta(_theta + rhs._theta);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Arithmetic operator overload for additions
|
||||
* @param lhs First addend
|
||||
* @param rhs Second addend
|
||||
*/
|
||||
friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2& rhs)
|
||||
{
|
||||
return lhs += rhs;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compound assignment operator (subtraction)
|
||||
* @param rhs value to subtract
|
||||
*/
|
||||
PoseSE2& operator-=(const PoseSE2& rhs)
|
||||
{
|
||||
_position -= rhs._position;
|
||||
_theta = g2o::normalize_theta(_theta - rhs._theta);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Arithmetic operator overload for subtractions
|
||||
* @param lhs First term
|
||||
* @param rhs Second term
|
||||
*/
|
||||
friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2& rhs)
|
||||
{
|
||||
return lhs -= rhs;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Multiply pose with scalar and return copy without normalizing theta
|
||||
* This operator is useful for calculating velocities ...
|
||||
* @param pose pose to scale
|
||||
* @param scalar factor to multiply with
|
||||
* @warning theta is not normalized after multiplying
|
||||
*/
|
||||
friend PoseSE2 operator*(PoseSE2 pose, double scalar)
|
||||
{
|
||||
pose._position *= scalar;
|
||||
pose._theta *= scalar;
|
||||
return pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Multiply pose with scalar and return copy without normalizing theta
|
||||
* This operator is useful for calculating velocities ...
|
||||
* @param scalar factor to multiply with
|
||||
* @param pose pose to scale
|
||||
* @warning theta is not normalized after multiplying
|
||||
*/
|
||||
friend PoseSE2 operator*(double scalar, PoseSE2 pose)
|
||||
{
|
||||
pose._position *= scalar;
|
||||
pose._theta *= scalar;
|
||||
return pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Output stream operator
|
||||
* @param stream output stream
|
||||
* @param pose to be used
|
||||
*/
|
||||
friend std::ostream& operator<< (std::ostream& stream, const PoseSE2& pose)
|
||||
{
|
||||
stream << "x: " << pose._position[0] << " y: " << pose._position[1] << " theta: " << pose._theta;
|
||||
return stream;
|
||||
}
|
||||
|
||||
///@}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
Eigen::Vector2d _position;
|
||||
double _theta;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
} // namespace teb_local_planner
|
||||
|
||||
#endif // POSE_SE2_H_
|
||||
Reference in New Issue
Block a user