costmap_3d: lint fixes

Various white space, include order, comment, and other lint fixes.

costmap_3d/include/costmap_3d/costmap_3d.h

@@ -38,8 +38,10 @@
 #define COSTMAP_3D_COSTMAP_3D_H_
 
 #include <memory>
-#include <octomap/octomap.h>
+#include <string>
+
 #include <geometry_msgs/Point.h>
+#include <octomap/octomap.h>
 
 namespace costmap_3d
 {
@@ -69,9 +71,9 @@ using Costmap3DIndexEntryType = octomap::key_type;
 class Costmap3D : public octomap::OcTree
 {
 public:
-  Costmap3D(double resolution, unsigned int depth = DEFAULT_DEPTH);
+  explicit Costmap3D(double resolution, unsigned int depth = DEFAULT_DEPTH);
   // Load a Costmap3D from a path to a binary octomap file
-  Costmap3D(std::string path);
+  explicit Costmap3D(std::string path);
   Costmap3D(const Costmap3D& rhs);
 
   // Return a new Costmap3D contatinaing only the nonlethal cells.

costmap_3d/include/costmap_3d/costmap_3d_publisher.h

@@ -37,6 +37,8 @@
 #ifndef COSTMAP_3D_COSTMAP_3D_PUBLISHER_H_
 #define COSTMAP_3D_COSTMAP_3D_PUBLISHER_H_
 
+#include <string>
+
 #include <ros/ros.h>
 #include <costmap_3d/layered_costmap_3d.h>
 #include <octomap_msgs/Octomap.h>
@@ -63,7 +65,8 @@ class Costmap3DPublisher
 
   void connectCallback(const ros::SingleSubscriberPublisher& pub);
   octomap_msgs::OctomapPtr createMapMessage(const Costmap3D& map);
-  octomap_msgs::OctomapUpdatePtr createMapUpdateMessage(const Costmap3D& value_map, const Costmap3D& bounds_map, bool first_map=false);
+  octomap_msgs::OctomapUpdatePtr createMapUpdateMessage(
+      const Costmap3D& value_map, const Costmap3D& bounds_map, bool first_map = false);
 
   std::string update_complete_id;
   ros::NodeHandle nh_;

costmap_3d/include/costmap_3d/costmap_3d_query.h

@@ -37,13 +37,16 @@
 #ifndef COSTMAP_3D_COSTMAP_3D_QUERY_H_
 #define COSTMAP_3D_COSTMAP_3D_QUERY_H_
 
-#include <string>
-#include <memory>
-#include <unordered_map>
-#include <limits>
+#include <algorithm>
 #include <atomic>
 #include <cmath>
+#include <limits>
+#include <memory>
 #include <shared_mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
 #include <Eigen/Dense>
 #include <fcl/geometry/bvh/BVH_model.h>
 #include <fcl/geometry/shape/utility.h>
@@ -116,13 +119,16 @@ class Costmap3DQuery
   static constexpr QueryRegion ALL = costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_REGION_ALL;
   static constexpr QueryRegion LEFT = costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_REGION_LEFT;
   static constexpr QueryRegion RIGHT = costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_REGION_RIGHT;
-  static constexpr QueryRegion RECTANGULAR_PRISM = costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_REGION_RECTANGULAR_PRISM;
+  static constexpr QueryRegion RECTANGULAR_PRISM =
+    costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_REGION_RECTANGULAR_PRISM;
   static constexpr QueryRegion MAX = RECTANGULAR_PRISM+1;
 
   /// What kind of obstacles to consider for the query.
   using QueryObstacles = uint8_t;
-  static constexpr QueryObstacles LETHAL_ONLY = costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_OBSTACLES_LETHAL_ONLY;
-  static constexpr QueryObstacles NONLETHAL_ONLY = costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_OBSTACLES_NONLETHAL_ONLY;
+  static constexpr QueryObstacles LETHAL_ONLY =
+    costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_OBSTACLES_LETHAL_ONLY;
+  static constexpr QueryObstacles NONLETHAL_ONLY =
+    costmap_3d_msgs::GetPlanCost3DService::Request::COST_QUERY_OBSTACLES_NONLETHAL_ONLY;
   static constexpr QueryObstacles OBSTACLES_MAX = NONLETHAL_ONLY+1;
 
   /** @brief Get the cost to put the robot base at the given pose.
@@ -252,7 +258,8 @@ class Costmap3DQuery
   };
 
   /** @brief Alternate footprintDistance interface taking a DistanceOptions */
-  virtual double footprintDistance(const geometry_msgs::Pose& pose, const DistanceOptions& opts, DistanceResult* result = nullptr);
+  virtual double footprintDistance(
+      const geometry_msgs::Pose& pose, const DistanceOptions& opts, DistanceResult* result = nullptr);
 
   /** @brief Recalculate footprintDistance with a previous result at a new pose. */
   virtual double footprintDistance(const geometry_msgs::Pose& pose, const DistanceResult& result);
@@ -467,7 +474,12 @@ class Costmap3DQuery
   class DistanceCacheKey
   {
   public:
-    DistanceCacheKey(const geometry_msgs::Pose& pose, QueryRegion query_region, bool query_obstacles, int bins_per_meter, int bins_per_rotation)
+    DistanceCacheKey(
+        const geometry_msgs::Pose& pose,
+        QueryRegion query_region,
+        bool query_obstacles,
+        int bins_per_meter,
+        int bins_per_rotation)
     {
       // If bins_per_meter or bins_per_rotation are zero, the cache is disabled.
       // The key is allowed to be calculated anyway as an exact key (and this
@@ -546,7 +558,7 @@ class Costmap3DQuery
       // Make the hash SIMD friendly by using primes and addition instead of
       // hash_combine which must be done sequentially.
       uint64_t rv = 0;
-      for (unsigned i=0; i<8; ++i)
+      for (unsigned i=0; i < 8; ++i)
       {
         rv += u[i].uint * primes[i];
       }
@@ -589,14 +601,14 @@ class Costmap3DQuery
       mesh_triangle_id = rhs.mesh_triangle_id;
       return *this;
     }
-    DistanceCacheEntry(const OcTreeMeshSolver<FCLSolver>::DistanceResult& result)
+    explicit DistanceCacheEntry(const OcTreeMeshSolver<FCLSolver>::DistanceResult& result)
         : distance(result.min_distance),
           octomap_box(result.octomap_box),
           octomap_box_center(result.octomap_box_center),
           mesh_triangle_id(result.mesh_triangle_id)
     {
     }
-    DistanceCacheEntry(const DistanceResult& result)
+    explicit DistanceCacheEntry(const DistanceResult& result)
         : octomap_box(result.octomap_box),
           octomap_box_center(result.octomap_box_center),
           mesh_triangle_id(result.mesh_triangle_id)
@@ -625,7 +637,10 @@ class Costmap3DQuery
     {
       return mesh_triangle_id >= 0;
     }
-    bool getCostmapIndexAndDepth(const octomap::OcTreeSpace& octree_space, Costmap3DIndex* index, unsigned int* depth) const
+    bool getCostmapIndexAndDepth(
+        const octomap::OcTreeSpace& octree_space,
+        Costmap3DIndex* index,
+        unsigned int* depth) const
     {
       if (*this)
       {
@@ -661,7 +676,8 @@ class Costmap3DQuery
   {
   public:
     // Construct a region of interest at a pose. Internally store the set of halfspaces.
-    RegionsOfInterestAtPose(QueryRegion query_region, const QueryRegionScale& query_region_scale, const geometry_msgs::Pose& pose)
+    RegionsOfInterestAtPose(
+        QueryRegion query_region, const QueryRegionScale& query_region_scale, const geometry_msgs::Pose& pose)
     {
       if (query_region == LEFT || query_region == RIGHT)
       {
@@ -675,7 +691,7 @@ class Costmap3DQuery
         {
           normal << 0.0, -1.0, 0.0;
         }
-        else if(query_region == RIGHT)
+        else if (query_region == RIGHT)
         {
           normal << 0.0, 1.0, 0.0;
         }
@@ -746,8 +762,10 @@ class Costmap3DQuery
   double handleDistanceInteriorCollisions(
       const DistanceCacheEntry& cache_entry,
       const geometry_msgs::Pose& pose);
-  using DistanceCache = std::unordered_map<DistanceCacheKey, DistanceCacheEntry, DistanceCacheKeyHash, DistanceCacheKeyEqual>;
-  using ExactDistanceCache = std::unordered_map<DistanceCacheKey, DistanceCacheEntry, DistanceCacheKeyHash, DistanceCacheKeyEqual>;
+  using DistanceCache =
+    std::unordered_map<DistanceCacheKey, DistanceCacheEntry, DistanceCacheKeyHash, DistanceCacheKeyEqual>;
+  using ExactDistanceCache =
+    std::unordered_map<DistanceCacheKey, DistanceCacheEntry, DistanceCacheKeyHash, DistanceCacheKeyEqual>;
   /**
    * The distance cache allows us to find a very good distance guess quickly.
    * The cache memorizes to a hash table for a pose rounded to the number of
@@ -821,7 +839,7 @@ class Costmap3DQuery
   // The write lock will be held when resetting these so they all reset
   // atomically
   std::atomic<unsigned int> queries_since_clear_;
-  std::atomic<unsigned int> empties_since_clear_; // an empty is a query on an empty costmap
+  std::atomic<unsigned int> empties_since_clear_;  // an empty is a query on an empty costmap
   std::atomic<unsigned int> hits_since_clear_;
   std::atomic<unsigned int> fast_milli_hits_since_clear_;
   std::atomic<unsigned int> slow_milli_hits_since_clear_;

costmap_3d/include/costmap_3d/costmap_3d_ros.h

@@ -37,11 +37,13 @@
 #ifndef COSTMAP_3D_COSTMAP_3D_ROS_H_
 #define COSTMAP_3D_COSTMAP_3D_ROS_H_
 
-#include <string>
 #include <map>
 #include <memory>
 #include <mutex>
+#include <set>
+#include <string>
 #include <tuple>
+
 #include <ros/ros.h>
 #include <actionlib/server/simple_action_server.h>
 #include <costmap_2d/costmap_2d_ros.h>
@@ -128,7 +130,10 @@ class Costmap3DROS : public costmap_2d::Costmap2DROS
   using super::resetBoundingBox;
 
   /** @brief Reset the costmap within the given axis-aligned bounding box in world coordinates for the given layers. */
-  virtual void resetBoundingBox(geometry_msgs::Point min, geometry_msgs::Point max, const std::set<std::string>& layers);
+  virtual void resetBoundingBox(
+      geometry_msgs::Point min,
+      geometry_msgs::Point max,
+      const std::set<std::string>& layers);
 
   /** @brief Get the cost to put the robot base at the given pose in the 3D costmap.
    *
@@ -246,17 +251,23 @@ class Costmap3DROS : public costmap_2d::Costmap2DROS
   // for a buffered query to a single copy of the costmap, serialize
   // the ROS service and action server.
   std::mutex service_mutex_;
-  void getPlanCost3DActionCallback(const actionlib::SimpleActionServer<costmap_3d_msgs::GetPlanCost3DAction>::GoalConstPtr& goal);
-  bool getPlanCost3DServiceCallback(costmap_3d_msgs::GetPlanCost3DService::Request& request, costmap_3d_msgs::GetPlanCost3DService::Response& response);
+  void getPlanCost3DActionCallback(
+      const actionlib::SimpleActionServer<costmap_3d_msgs::GetPlanCost3DAction>::GoalConstPtr& goal);
+  bool getPlanCost3DServiceCallback(
+      costmap_3d_msgs::GetPlanCost3DService::Request& request,
+      costmap_3d_msgs::GetPlanCost3DService::Response& response);
   template <typename RequestType, typename ResponseType>
   void processPlanCost3D(RequestType& request, ResponseType& response);
   // Should be holding the service mutex, but not the costmap mutex
   virtual std::shared_ptr<Costmap3DQuery> getBufferedQueryForService(
           const std::string& footprint_mesh_resource = "",
           double padding = NAN);
 
-  void rayQuery3DActionCallback(const actionlib::SimpleActionServer<costmap_3d_msgs::RayQuery3DAction>::GoalConstPtr& goal);
-  bool rayQuery3DServiceCallback(costmap_3d_msgs::RayQuery3DService::Request& request, costmap_3d_msgs::RayQuery3DService::Response& response);
+  void rayQuery3DActionCallback(
+      const actionlib::SimpleActionServer<costmap_3d_msgs::RayQuery3DAction>::GoalConstPtr& goal);
+  bool rayQuery3DServiceCallback(
+      costmap_3d_msgs::RayQuery3DService::Request& request,
+      costmap_3d_msgs::RayQuery3DService::Response& response);
   template <typename RequestType, typename ResponseType>
   bool processRayQuery3D(RequestType& request, ResponseType& response);
 
@@ -288,7 +299,7 @@ class Costmap3DROS : public costmap_2d::Costmap2DROS
       {
         return false;
       }
-      for (unsigned int i=0; i<3; ++i)
+      for (unsigned int i=0; i < 3; ++i)
       {
         if (lhs2(i) < rhs2(i))
         {
@@ -328,9 +339,8 @@ class Costmap3DROS : public costmap_2d::Costmap2DROS
 
   std::string footprint_mesh_resource_;
   double footprint_3d_padding_;
-
 };
-// class Costmap3DROS
+
 }  // namespace costmap_3d
 
 #endif  // COSTMAP_3D_COSTMAP_3D_ROS_H_

costmap_3d/include/costmap_3d/costmap_3d_to_2d_layer.h

@@ -78,6 +78,6 @@ class Costmap3DTo2DLayer : public costmap_2d::CostmapLayer
   LayeredCostmap3D* layered_costmap_3d_;
 };
 
-}  // namespace costmap_2d
+}  // namespace costmap_3d
 
 #endif  // COSTMAP_3D_COSTMAP_3D_TO_2D_LAYER_H_

costmap_3d/include/costmap_3d/costmap_layer_3d.h

@@ -134,7 +134,7 @@ class CostmapLayer3D : public Layer3D
   /** @brief Touch the given key at the given depth.
    * This layer must be holding the lock to make this call. */
   virtual void touchKeyAtDepth(const octomap::OcTreeKey& key,
-                               unsigned int depth=std::numeric_limits<unsigned int>::max());
+                               unsigned int depth = std::numeric_limits<unsigned int>::max());
 
   /** @brief Update the cells in binary fashion from the given values and bounds.
    * Note: in this version, the value and bounds trees will have their depth
@@ -154,12 +154,12 @@ class CostmapLayer3D : public Layer3D
   /** @brief Update the cell at the given point.
    * If mark is true, mark the cell, otherwise clear it.
    * This layer must be holding the lock to make this call. */
-  virtual void updateCell(const geometry_msgs::Point& point, bool mark=false);
+  virtual void updateCell(const geometry_msgs::Point& point, bool mark = false);
 
   /** @brief Update the cell at the given key.
    * If mark is true, mark the cell, otherwise clear it.
    * This layer must be holding the lock to make this call. */
-  virtual void updateCell(const octomap::OcTreeKey& key, bool mark=false);
+  virtual void updateCell(const octomap::OcTreeKey& key, bool mark = false);
 
   /** @brief Mark the cell at the given point.
    * This layer must be holding the lock to make this call. */
@@ -203,7 +203,7 @@ class CostmapLayer3D : public Layer3D
   /** @brief Set the cell cost at the given key and depth.
    * This layer must be holding the lock to make this call. */
   virtual void setCellCostAtDepth(const octomap::OcTreeKey& key, Cost cost,
-                                  unsigned int depth=std::numeric_limits<unsigned int>::max());
+                                  unsigned int depth = std::numeric_limits<unsigned int>::max());
 
   /** @brief Set the cell cost at the given key.
    * This layer must be holding the lock to make this call. */