DuongTD/pose_se2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2026-04-07 09:54:43 +07:00
commit 14fac7843d
5 changed files with 779 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
CMakeLists.txt Normal file
View File

@@ -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()

56
README.md Normal file
View File

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

152
include/pose_se2/misc.h Normal file
View 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
View 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_

30
package.xml Normal file
View File

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