make optional the usage of approximations and add support for it on a…

…ll footprints

cfg/TebLocalPlannerReconfigure.cfg

@@ -390,4 +390,16 @@ 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
+)
+
 exit(gen.generate("teb_local_planner", "teb_local_planner", "TebLocalPlannerReconfigure"))

include/teb_local_planner/robot_footprint_model.h

@@ -43,6 +43,7 @@
 #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
@@ -63,7 +64,8 @@ class BaseRobotFootprintModel
   /**
     * @brief Default constructor of the abstract obstacle class
     */
-  BaseRobotFootprintModel()
+  BaseRobotFootprintModel() :
+    cfg_(nullptr)
   {
   }
 
@@ -110,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
@@ -299,7 +320,9 @@ class TwoCirclesRobotFootprint : public BaseRobotFootprintModel
     */
   virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
   {
-    Eigen::Vector2d dir = current_pose.orientationUnitVec();
+    double sin_th, cos_th;
+    cfg_->sincos(current_pose.theta(), sin_th, cos_th);
+    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);
@@ -314,7 +337,9 @@ 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, cos_th;
+    cfg_->sincos(current_pose.theta(), sin_th, cos_th);
+    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);
@@ -330,8 +355,10 @@ 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, cos_th;
+    cfg_->sincos(current_pose.theta(), sin_th, cos_th);
+    Eigen::Vector2d dir(cos_th, sin_th);
     if (front_radius_>0)
     {
       markers.push_back(visualization_msgs::Marker());
@@ -518,7 +545,7 @@ class LineRobotFootprint : public BaseRobotFootprintModel
   void transformToWorld(const PoseSE2& current_pose, Eigen::Vector2d& line_start_world, Eigen::Vector2d& line_end_world) const
   {
     double sin_th, cos_th;
-    teb_local_planner::sincos_approx(current_pose.theta(), sin_th, cos_th);
+    cfg_->sincos(current_pose.theta(), sin_th, cos_th);
     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();
@@ -663,8 +690,11 @@ 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 cos_th;
+    double sin_th;
+
+    cfg_->sincos(current_pose.theta(), sin_th, cos_th);
+
     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
@@ -215,6 +216,12 @@ class TebConfig
     double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations
   } 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.
@@ -366,6 +373,9 @@ class TebConfig
     recovery.oscillation_recovery_min_duration = 10;
     recovery.oscillation_filter_duration = 10;
 
+    // Recovery
+
+    performance.use_sin_cos_approximation = false;
 
   }
 
@@ -399,6 +409,27 @@ class TebConfig
    */
   void checkDeprecated(const ros::NodeHandle& nh) const;
 
+  template <typename T>
+  inline void sincos(T angle, T& sin, T& cos) const
+  {
+    if (performance.use_sin_cos_approximation)
+      teb_local_planner::sincos_approx(angle, sin, cos);
+    else
+    {
+      sin = std::sin(angle);
+      cos = std::cos(angle);
+    }
+  }
+
+  template <typename T>
+  inline void sin(T angle, T& sin) const
+  {
+    if (performance.use_sin_cos_approximation)
+      teb_local_planner::sin_approx(angle, sin);
+    else
+      sin = 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

@@ -87,6 +87,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

@@ -168,6 +168,9 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
   nh.param("oscillation_recovery_min_duration", recovery.oscillation_recovery_min_duration, recovery.oscillation_recovery_min_duration);
   nh.param("oscillation_filter_duration", recovery.oscillation_filter_duration, recovery.oscillation_filter_duration);
 
+  // Performance
+  nh.param("use_sin_cos_approximation", performance.use_sin_cos_approximation, performance.use_sin_cos_approximation);
+
   checkParameters();
   checkDeprecated(nh);
 }
@@ -274,6 +277,10 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
 
   recovery.shrink_horizon_backup = cfg.shrink_horizon_backup;
   recovery.oscillation_recovery = cfg.oscillation_recovery;
+
+  // Performance
+
+  performance.use_sin_cos_approximation = cfg.use_sin_cos_approximation;
 
   checkParameters();
 }

src/teb_local_planner_ros.cpp

@@ -102,6 +102,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)