Improve the performance of computing sin and cos functions

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")
 

include/teb_local_planner/misc.h

@@ -147,6 +147,35 @@ inline const T& get_const_reference(const T* ptr) {return *ptr;}
 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;}
 
+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 */

include/teb_local_planner/robot_footprint_model.h

@@ -42,6 +42,8 @@
 
 #include <teb_local_planner/pose_se2.h>
 #include <teb_local_planner/obstacles.h>
+#include <teb_local_planner/misc.h>
+#include <teb_local_planner/teb_config.h>
 #include <visualization_msgs/Marker.h>
 
 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<visualization_msgs::Marker>& 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<vertices_.size(); ++i)
     {
       polygon_world[i].x() = current_pose.x() + cos_th * vertices_[i].x() - sin_th * vertices_[i].y();

include/teb_local_planner/teb_config.h

@@ -45,6 +45,7 @@
 #include <Eigen/StdVector>
 
 #include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>
+#include <teb_local_planner/misc.h>
 
 
 // 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 <typename T>
+  T cos(T angle) const
+  {
+    return performance.use_sin_cos_approximation ? teb_local_planner::cos_fast(angle) : std::cos(angle);
+  }
+
+  template <typename T>
+  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
    */

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<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));

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;

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();
 }

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)

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)