commit 14fac7843dce9529c8d3a274b8031c73584799f5 Author: duongtd Date: Tue Apr 7 09:54:43 2026 +0700 first commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f890677 --- /dev/null +++ b/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 + $ + $ + ) + + # 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 + $ + $ + ) + + # 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() diff --git a/README.md b/README.md new file mode 100644 index 0000000..0f01951 --- /dev/null +++ b/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. + +Buy Me A Coffee + +## 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. + + + + +## 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 + + diff --git a/include/pose_se2/misc.h b/include/pose_se2/misc.h new file mode 100644 index 0000000..414154d --- /dev/null +++ b/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 +#include +#include + + +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& angles) +{ + double x=0, y=0; + for (std::vector::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) +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 +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 +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 +inline const T& get_const_reference(const T& val, typename boost::disable_if >::type* dummy = 0) {return val;} + +} // namespace teb_local_planner + +#endif /* MISC_H */ diff --git a/include/pose_se2/pose_se2.h b/include/pose_se2/pose_se2.h new file mode 100755 index 0000000..91da4ff --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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& 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_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..512e07e --- /dev/null +++ b/package.xml @@ -0,0 +1,30 @@ + + pose_se2 + + 0.9.1 + + + 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. + + + + Christoph Rösmann + + BSD + + http://wiki.ros.org/pose_se2 + + Christoph Rösmann + + catkin + + robot_geometry_msgs + robot_geometry_msgs + + +