first commit

CMakeLists.txt

@@ -0,0 +1,146 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(pose_se2 VERSION 1.0.0 LANGUAGES CXX)
+
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building pose_se2 with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building pose_se2 with Standalone CMake")
+endif()
+
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# find_package(G2O REQUIRED)
+
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+  set(PACKAGES_DIR  
+    robot_geometry_msgs
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_geometry_msgs
+  )
+
+  catkin_package(
+    INCLUDE_DIRS include
+    # LIBRARIES không cần vì đây là header-only library
+    CATKIN_DEPENDS robot_geometry_msgs
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+  )
+endif()
+
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/pose_se2/*.h")
+
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${catkin_LIBRARIES}
+      Eigen3::Eigen
+  )
+
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+      Eigen3::Eigen
+  )
+
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Headers found:")
+  foreach(hdr ${HEADERS})
+    message(STATUS "  - ${hdr}")
+  endforeach()
+  message(STATUS "Dependencies: robot_geometry_msgs")
+  message(STATUS "=================================")
+endif()

README.md

@@ -0,0 +1,56 @@
+teb_local_planner ROS Package
+=============================
+
+The teb_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. 
+The underlying method called Timed Elastic Band locally optimizes the robot's trajectory with respect to trajectory execution time, 
+separation from obstacles and compliance with kinodynamic constraints at runtime.
+
+Refer to http://wiki.ros.org/teb_local_planner for more information and tutorials.
+
+Build status of the *melodic-devel* branch:
+- ROS Buildfarm (Melodic): [![Melodic Status](http://build.ros.org/buildStatus/icon?job=Mdev__teb_local_planner__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__teb_local_planner__ubuntu_bionic_amd64/)
+
+
+## Citing the Software
+
+*Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the planner for your own research:*
+
+- C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.
+- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79.
+- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143.
+- C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015.
+- C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017.
+
+<a href="https://www.buymeacoffee.com/croesmann" target="_blank"><img src="https://cdn.buymeacoffee.com/buttons/lato-black.png" alt="Buy Me A Coffee" height="31px" width="132px" ></a>
+
+## Videos
+
+The left of the following videos presents features of the package and shows examples from simulation and real robot situations.
+Some spoken explanations are included in the audio track of the video. 
+The right one demonstrates features introduced in version 0.2 (supporting car-like robots and costmap conversion). Please watch the left one first.
+
+<a href="http://www.youtube.com/watch?feature=player_embedded&v=e1Bw6JOgHME" target="_blank"><img src="http://img.youtube.com/vi/e1Bw6JOgHME/0.jpg" 
+alt="teb_local_planner - An Optimal Trajectory Planner for Mobile Robots" width="240" height="180" border="10" /></a>
+<a href="http://www.youtube.com/watch?feature=player_embedded&v=o5wnRCzdUMo" target="_blank"><img src="http://img.youtube.com/vi/o5wnRCzdUMo/0.jpg" 
+alt="teb_local_planner - Car-like Robots and Costmap Conversion" width="240" height="180" border="10" /></a>
+
+## License
+
+The *teb_local_planner* package is licensed under the BSD license.
+It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
+
+Some third-party dependencies are included that are licensed under different terms:
+ - *Eigen*, MPL2 license, http://eigen.tuxfamily.org
+ - *libg2o* / *g2o* itself is licensed under BSD, but the enabled *csparse_extension* is licensed under LGPL3+, 
+   https://github.com/RainerKuemmerle/g2o. [*CSparse*](http://www.cise.ufl.edu/research/sparse/CSparse/) is included as part of the *SuiteSparse* collection, http://www.suitesparse.com. 
+ - *Boost*, Boost Software License, http://www.boost.org
+
+All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.
+
+## Requirements
+
+Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*:
+
+    rosdep install teb_local_planner
+
+

include/pose_se2/misc.h

@@ -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 */

include/pose_se2/pose_se2.h

@@ -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_

package.xml

@@ -0,0 +1,30 @@
+<package>
+  <name>pose_se2</name>
+
+  <version>0.9.1</version>
+  
+  <description>
+    The pose_se2 package implements a plugin
+    to the base_local_planner of the 2D navigation stack.
+    The underlying method called Timed Elastic Band locally optimizes
+    the robot's trajectory with respect to trajectory execution time,
+    separation from obstacles and compliance with kinodynamic constraints at runtime.	
+  </description>
+
+
+  <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/pose_se2</url>
+
+  <author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+  <!-- <build_depend>libg2o</build_depend>
+  <run_depend>libg2o</run_depend> -->
+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+
+ 
+</package>