diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index 983930e4..6fa898a8 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -412,6 +412,18 @@ grp_recovery.add("shrink_horizon_backup", bool_t, 0, grp_recovery.add("oscillation_recovery", bool_t, 0, "Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).", True) + + +# Performance +grp_performance = gen.add_group("Performance", type="tab") + +grp_performance.add( + "use_sin_cos_approximation", + bool_t, + 0, + "Use sin and cos approximations to improve performance. The maximum absolute error for these approximations is 1e-3.", + False +) grp_recovery_divergence = grp_recovery.add_group("Divergence Detection", type="hide") diff --git a/include/teb_local_planner/misc.h b/include/teb_local_planner/misc.h index 09f055af..d384862f 100644 --- a/include/teb_local_planner/misc.h +++ b/include/teb_local_planner/misc.h @@ -147,6 +147,35 @@ inline const T& get_const_reference(const T* ptr) {return *ptr;} template inline const T& get_const_reference(const T& val, typename boost::disable_if >::type* dummy = 0) {return val;} +inline float sin_fast(float angle) +{ + // Code borrowed from https://github.com/kennyalive/fast-sine-cosine + constexpr float PI = 3.14159265358f; + constexpr float PI2 = 2 * PI; + constexpr float B = 4.0f / PI; + constexpr float C = -4.0f / (PI * PI); + constexpr float P = 0.225f; + + angle = std::fmod(angle, PI2); + if (angle > PI) + angle -= PI2; + + angle = B * angle + C * angle * (angle < 0 ? -angle : angle); + return P * (angle * (angle < 0 ? -angle : angle) - angle) + angle; +} + +inline float cos_fast(float angle) +{ + // Code borrowed from https://github.com/kennyalive/fast-sine-cosine + constexpr float PI = 3.14159265358f; + constexpr float PI2 = 2 * PI; + + angle = (angle > 0) ? -angle : angle; + angle += PI/2; + + return sin_fast(angle); +} + } // namespace teb_local_planner #endif /* MISC_H */ diff --git a/include/teb_local_planner/robot_footprint_model.h b/include/teb_local_planner/robot_footprint_model.h index 5466ba43..0b1848dd 100644 --- a/include/teb_local_planner/robot_footprint_model.h +++ b/include/teb_local_planner/robot_footprint_model.h @@ -42,6 +42,8 @@ #include #include +#include +#include #include namespace teb_local_planner @@ -62,7 +64,8 @@ class BaseRobotFootprintModel /** * @brief Default constructor of the abstract obstacle class */ - BaseRobotFootprintModel() + BaseRobotFootprintModel() : + cfg_(nullptr) { } @@ -109,7 +112,26 @@ class BaseRobotFootprintModel */ virtual double getInscribedRadius() = 0; + /** + * @brief Assign the TebConfig class for parameters. + * @param cfg TebConfig class + */ + inline void setTEBConfiguration(const TebConfig& config) + { + cfg_ = &config; + } + + /** + * @brief Assign the TebConfig class for parameters. + * @param cfg TebConfig class + */ + inline void setTEBConfiguration(const TebConfig * const config) + { + cfg_ = config; + } +protected: + const TebConfig* cfg_; //!< Store TebConfig class for parameters public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -298,7 +320,10 @@ class TwoCirclesRobotFootprint : public BaseRobotFootprintModel */ virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const { - Eigen::Vector2d dir = current_pose.orientationUnitVec(); + double sin_th = cfg_->sin(current_pose.theta()); + double cos_th = cfg_->cos(current_pose.theta()); + + Eigen::Vector2d dir(cos_th, sin_th); double dist_front = obstacle->getMinimumDistance(current_pose.position() + front_offset_*dir) - front_radius_; double dist_rear = obstacle->getMinimumDistance(current_pose.position() - rear_offset_*dir) - rear_radius_; return std::min(dist_front, dist_rear); @@ -313,7 +338,10 @@ class TwoCirclesRobotFootprint : public BaseRobotFootprintModel */ virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const { - Eigen::Vector2d dir = current_pose.orientationUnitVec(); + double sin_th = cfg_->sin(current_pose.theta()); + double cos_th = cfg_->cos(current_pose.theta()); + + Eigen::Vector2d dir(cos_th, sin_th); double dist_front = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() + front_offset_*dir, t) - front_radius_; double dist_rear = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() - rear_offset_*dir, t) - rear_radius_; return std::min(dist_front, dist_rear); @@ -329,8 +357,11 @@ class TwoCirclesRobotFootprint : public BaseRobotFootprintModel * @param color Color of the footprint */ virtual void visualizeRobot(const PoseSE2& current_pose, std::vector& markers, const std_msgs::ColorRGBA& color) const - { - Eigen::Vector2d dir = current_pose.orientationUnitVec(); + { + double sin_th = cfg_->sin(current_pose.theta()); + double cos_th = cfg_->cos(current_pose.theta()); + + Eigen::Vector2d dir(cos_th, sin_th); if (front_radius_>0) { markers.push_back(visualization_msgs::Marker()); @@ -516,8 +547,9 @@ class LineRobotFootprint : public BaseRobotFootprintModel */ void transformToWorld(const PoseSE2& current_pose, Eigen::Vector2d& line_start_world, Eigen::Vector2d& line_end_world) const { - double cos_th = std::cos(current_pose.theta()); - double sin_th = std::sin(current_pose.theta()); + double sin_th = cfg_->sin(current_pose.theta()); + double cos_th = cfg_->cos(current_pose.theta()); + line_start_world.x() = current_pose.x() + cos_th * line_start_.x() - sin_th * line_start_.y(); line_start_world.y() = current_pose.y() + sin_th * line_start_.x() + cos_th * line_start_.y(); line_end_world.x() = current_pose.x() + cos_th * line_end_.x() - sin_th * line_end_.y(); @@ -662,8 +694,9 @@ class PolygonRobotFootprint : public BaseRobotFootprintModel */ void transformToWorld(const PoseSE2& current_pose, Point2dContainer& polygon_world) const { - double cos_th = std::cos(current_pose.theta()); - double sin_th = std::sin(current_pose.theta()); + double sin_th = cfg_->sin(current_pose.theta()); + double cos_th = cfg_->cos(current_pose.theta()); + for (std::size_t i=0; i #include +#include // Definitions @@ -222,6 +223,12 @@ class TebConfig int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged. } recovery; //!< Parameters related to recovery and backup strategies + //! Performance enhancement related parameters + struct Performance + { + bool use_sin_cos_approximation; //!< Use sin and cos approximations to improve performance. The maximum absolute error for these approximations is 1e-3. + } performance; + /** * @brief Construct the TebConfig using default values. @@ -376,6 +383,9 @@ class TebConfig recovery.oscillation_recovery_min_duration = 10; recovery.oscillation_filter_duration = 10; + // Recovery + + performance.use_sin_cos_approximation = false; } @@ -409,6 +419,18 @@ class TebConfig */ void checkDeprecated(const ros::NodeHandle& nh) const; + template + T cos(T angle) const + { + return performance.use_sin_cos_approximation ? teb_local_planner::cos_fast(angle) : std::cos(angle); + } + + template + T sin(T angle) const + { + return performance.use_sin_cos_approximation ? teb_local_planner::sin_fast(angle) : std::sin(angle); + } + /** * @brief Return the internal config mutex */ diff --git a/src/homotopy_class_planner.cpp b/src/homotopy_class_planner.cpp index 4b8633e1..d405925c 100644 --- a/src/homotopy_class_planner.cpp +++ b/src/homotopy_class_planner.cpp @@ -64,6 +64,7 @@ void HomotopyClassPlanner::initialize(const TebConfig& cfg, ObstContainer* obsta obstacles_ = obstacles; via_points_ = via_points; robot_model_ = robot_model; + robot_model_->setTEBConfiguration(cfg_); if (cfg_->hcp.simple_exploration) graph_search_ = boost::shared_ptr(new lrKeyPointGraph(*cfg_, this)); diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index ce303c52..c397ae1c 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -92,6 +92,7 @@ void TebOptimalPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacle cfg_ = &cfg; obstacles_ = obstacles; robot_model_ = robot_model; + robot_model_->setTEBConfiguration(cfg_); via_points_ = via_points; cost_ = HUGE_VAL; prefer_rotdir_ = RotType::none; diff --git a/src/teb_config.cpp b/src/teb_config.cpp index b8c5b17e..ddc0f1a6 100644 --- a/src/teb_config.cpp +++ b/src/teb_config.cpp @@ -175,6 +175,9 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) nh.param("divergence_detection", recovery.divergence_detection_enable, recovery.divergence_detection_enable); nh.param("divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared); + // Performance + nh.param("use_sin_cos_approximation", performance.use_sin_cos_approximation, performance.use_sin_cos_approximation); + checkParameters(); checkDeprecated(nh); } @@ -288,6 +291,8 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) recovery.divergence_detection_enable = cfg.divergence_detection_enable; recovery.divergence_detection_max_chi_squared = cfg.divergence_detection_max_chi_squared; + // Performance + performance.use_sin_cos_approximation = cfg.use_sin_cos_approximation; checkParameters(); } diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 7b8f3d7e..d1765682 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -107,6 +107,7 @@ void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm // create robot footprint/contour model for optimization RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh); + robot_model->setTEBConfiguration(cfg_); // create the planner instance if (cfg_.hcp.enable_homotopy_class_planning) diff --git a/src/test_optim_node.cpp b/src/test_optim_node.cpp index a5bc9e74..928d131a 100644 --- a/src/test_optim_node.cpp +++ b/src/test_optim_node.cpp @@ -148,6 +148,7 @@ int main( int argc, char** argv ) // Setup robot shape model RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n); + robot_model->setTEBConfiguration(config); // Setup planner (homotopy class planning or just the local teb planner) if (config.hcp.enable_homotopy_class_planning)