[MPPI] complete minor optimaization with floating point calculations (ros-navigation#3827)

* floating point calculations

* Update optimizer_unit_tests.cpp

* Update critics_tests.cpp

* Update critics_tests.cpp

Signed-off-by: enricosutera <enricosutera@outlook.com>

Author: SteveMacenski

nav2_mppi_controller/CMakeLists.txt

@@ -47,7 +47,8 @@ if(COMPILER_SUPPORTS_FMA)
   add_compile_options(-mfma)
 endif()
 
-add_compile_options(-O3 -finline-limit=1000000 -ffp-contract=fast -ffast-math)
+# If building one the same hardware to be deployed on, try `-march=native`!
+add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
 
 add_library(mppi_controller SHARED
   src/controller.cpp

nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp

@@ -79,14 +79,14 @@ class ObstaclesCritic : public CriticFunction
     * @return double circumscribed cost, any higher than this and need to do full footprint collision checking
     * since some element of the robot could be in collision
     */
-  double findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
+  float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
 
 protected:
   nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
   collision_checker_{nullptr};
 
   bool consider_footprint_{true};
-  double collision_cost_{0};
+  float collision_cost_{0};
   float inflation_scale_factor_{0}, inflation_radius_{0};
 
   float possibly_inscribed_cost_;

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp

@@ -72,7 +72,7 @@ class PathAngleCritic : public CriticFunction
   void score(CriticData & data) override;
 
 protected:
-  double max_angle_to_furthest_{0};
+  float max_angle_to_furthest_{0};
   float threshold_to_consider_{0};
 
   size_t offset_from_furthest_{0};

nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp

@@ -24,10 +24,10 @@ namespace mppi::models
  */
 struct ControlConstraints
 {
-  double vx_max;
-  double vx_min;
-  double vy;
-  double wz;
+  float vx_max;
+  float vx_min;
+  float vy;
+  float wz;
 };
 
 /**
@@ -36,9 +36,9 @@ struct ControlConstraints
  */
 struct SamplingStd
 {
-  double vx;
-  double vy;
-  double wz;
+  float vx;
+  float vy;
+  float wz;
 };
 
 }  // namespace mppi::models

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -151,8 +151,8 @@ class PathHandler
   double max_robot_pose_search_dist_{0};
   double prune_distance_{0};
   double transform_tolerance_{0};
-  double inversion_xy_tolerance_{0.2};
-  double inversion_yaw_tolerance{0.4};
+  float inversion_xy_tolerance_{0.2};
+  float inversion_yaw_tolerance{0.4};
   bool enforce_path_inversion_{false};
   unsigned int inversion_locale_{0u};
 };

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -309,7 +309,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data)
   const auto dists = dx * dx + dy * dy;
 
   size_t max_id_by_trajectories = 0;
-  double min_distance_by_path = std::numeric_limits<float>::max();
+  float min_distance_by_path = std::numeric_limits<float>::max();
 
   for (size_t i = 0; i < dists.shape(0); i++) {
     size_t min_id_by_path = 0;
@@ -337,7 +337,7 @@ inline size_t findPathTrajectoryInitialPoint(const CriticData & data)
   const auto dy = data.path.y - data.trajectories.y(0, 0);
   const auto dists = dx * dx + dy * dy;
 
-  double min_distance_by_path = std::numeric_limits<float>::max();
+  float min_distance_by_path = std::numeric_limits<float>::max();
   size_t min_id = 0;
   for (size_t j = 0; j < dists.shape(0); j++) {
     if (dists(j) < min_distance_by_path) {
@@ -420,23 +420,23 @@ inline void setPathCostsIfNotSet(
  * @param forward_preference If reversing direction is valid
  * @return Angle between two points
  */
-inline double posePointAngle(
+inline float posePointAngle(
   const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference)
 {
-  double pose_x = pose.position.x;
-  double pose_y = pose.position.y;
-  double pose_yaw = tf2::getYaw(pose.orientation);
+  float pose_x = pose.position.x;
+  float pose_y = pose.position.y;
+  float pose_yaw = tf2::getYaw(pose.orientation);
 
-  double yaw = atan2(point_y - pose_y, point_x - pose_x);
+  float yaw = atan2f(point_y - pose_y, point_x - pose_x);
 
   // If no preference for forward, return smallest angle either in heading or 180 of heading
   if (!forward_preference) {
     return std::min(
-      abs(angles::shortest_angular_distance(yaw, pose_yaw)),
-      abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI))));
+      fabs(angles::shortest_angular_distance(yaw, pose_yaw)),
+      fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI))));
   }
 
-  return abs(angles::shortest_angular_distance(yaw, pose_yaw));
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
 }
 
 /**
@@ -447,21 +447,21 @@ inline double posePointAngle(
  * @param point_yaw Yaw of the point to consider along Z axis
  * @return Angle between two points
  */
-inline double posePointAngle(
+inline float posePointAngle(
   const geometry_msgs::msg::Pose & pose,
   double point_x, double point_y, double point_yaw)
 {
-  double pose_x = pose.position.x;
-  double pose_y = pose.position.y;
-  double pose_yaw = tf2::getYaw(pose.orientation);
+  float pose_x = pose.position.x;
+  float pose_y = pose.position.y;
+  float pose_yaw = tf2::getYaw(pose.orientation);
 
-  double yaw = atan2(point_y - pose_y, point_x - pose_x);
+  float yaw = atan2f(point_y - pose_y, point_x - pose_x);
 
-  if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) {
+  if (fabs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) {
     yaw = angles::normalize_angle(yaw + M_PI);
   }
 
-  return abs(angles::shortest_angular_distance(yaw, pose_yaw));
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
 }
 
 /**
@@ -650,17 +650,17 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
   // Iterating through the path to determine the position of the path inversion
   for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
     // We have two vectors for the dot product OA and AB. Determining the vectors.
-    double oa_x = path.poses[idx].pose.position.x -
+    float oa_x = path.poses[idx].pose.position.x -
       path.poses[idx - 1].pose.position.x;
-    double oa_y = path.poses[idx].pose.position.y -
+    float oa_y = path.poses[idx].pose.position.y -
       path.poses[idx - 1].pose.position.y;
-    double ab_x = path.poses[idx + 1].pose.position.x -
+    float ab_x = path.poses[idx + 1].pose.position.x -
       path.poses[idx].pose.position.x;
-    double ab_y = path.poses[idx + 1].pose.position.y -
+    float ab_y = path.poses[idx + 1].pose.position.y -
       path.poses[idx].pose.position.y;
 
     // Checking for the existance of cusp, in the path, using the dot product.
-    double dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
     if (dot_product < 0.0) {
       return idx + 1;
     }

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -32,7 +32,7 @@ void ObstaclesCritic::initialize()
   collision_checker_.setCostmap(costmap_);
   possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
 
-  if (possibly_inscribed_cost_ < 1) {
+  if (possibly_inscribed_cost_ < 1.0f) {
     RCLCPP_ERROR(
       logger_,
       "Inflation layer either not found or inflation is not set sufficiently for "
@@ -50,7 +50,7 @@ void ObstaclesCritic::initialize()
     "footprint" : "circular");
 }
 
-double ObstaclesCritic::findCircumscribedCost(
+float ObstaclesCritic::findCircumscribedCost(
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
 {
   double result = -1.0;
@@ -75,15 +75,15 @@ double ObstaclesCritic::findCircumscribedCost(
 
   if (!inflation_layer_found) {
     RCLCPP_WARN(
-      rclcpp::get_logger("computeCircumscribedCost"),
+      logger_,
       "No inflation layer found in costmap configuration. "
       "If this is an SE2-collision checking plugin, it cannot use costmap potential "
       "field to speed up collision checking by only checking the full footprint "
       "when robot is within possibly-inscribed radius of an obstacle. This may "
       "significantly slow down planning times and not avoid anything but absolute collisions!");
   }
 
-  return result;
+  return static_cast<float>(result);
 }
 
 float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost)
@@ -137,7 +137,7 @@ void ObstaclesCritic::score(CriticData & data)
       }
 
       // Cannot process repulsion if inflation layer does not exist
-      if (inflation_radius_ == 0 || inflation_scale_factor_ == 0) {
+      if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {
         continue;
       }
 
@@ -197,7 +197,9 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
   }
   cost = collision_checker_.pointCost(x_i, y_i);
 
-  if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) {
+  if (consider_footprint_ &&
+    (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
+  {
     cost = static_cast<float>(collision_checker_.footprintCostAtPose(
         x, y, theta, costmap_ros_->getRobotFootprint()));
     collision_cost.using_footprint = true;

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -89,7 +89,7 @@ void PathAlignCritic::score(CriticData & data)
   }
 
   float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f;
-  double min_dist_sq = std::numeric_limits<float>::max();
+  float min_dist_sq = std::numeric_limits<float>::max();
   size_t min_s = 0;
 
   for (size_t t = 0; t < batch_size; ++t) {

nav2_mppi_controller/src/noise_generator.cpp

@@ -94,14 +94,14 @@ void NoiseGenerator::generateNoisedControls()
   auto & s = settings_;
 
   xt::noalias(noises_vx_) = xt::random::randn<float>(
-    {s.batch_size, s.time_steps}, 0.0,
+    {s.batch_size, s.time_steps}, 0.0f,
     s.sampling_std.vx);
   xt::noalias(noises_wz_) = xt::random::randn<float>(
-    {s.batch_size, s.time_steps}, 0.0,
+    {s.batch_size, s.time_steps}, 0.0f,
     s.sampling_std.wz);
   if (is_holonomic_) {
     xt::noalias(noises_vy_) = xt::random::randn<float>(
-      {s.batch_size, s.time_steps}, 0.0,
+      {s.batch_size, s.time_steps}, 0.0f,
       s.sampling_std.vy);
   }
 }

nav2_mppi_controller/src/path_handler.cpp

@@ -166,8 +166,8 @@ bool PathHandler::transformPose(
 double PathHandler::getMaxCostmapDist()
 {
   const auto & costmap = costmap_->getCostmap();
-  return std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) *
-         costmap->getResolution() / 2.0;
+  return static_cast<double>(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) *
+         costmap->getResolution() * 0.50;
 }
 
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
@@ -190,11 +190,11 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam
 {
   // Keep full path if we are within tolerance of the inversion pose
   const auto last_pose = global_plan_up_to_inversion_.poses.back();
-  double distance = std::hypot(
+  float distance = hypotf(
     robot_pose.pose.position.x - last_pose.pose.position.x,
     robot_pose.pose.position.y - last_pose.pose.position.y);
 
-  double angle_distance = angles::shortest_angular_distance(
+  float angle_distance = angles::shortest_angular_distance(
     tf2::getYaw(robot_pose.pose.orientation),
     tf2::getYaw(last_pose.pose.orientation));