diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp
index 4514f43d6c..bcb5c7d97c 100644
--- a/moveit_core/collision_detection/src/collision_env.cpp
+++ b/moveit_core/collision_detection/src/collision_env.cpp
@@ -42,7 +42,7 @@
 // Logger
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_robot");
 
-static inline bool validateScale(double scale)
+static inline bool validateScale(const double scale)
 {
   if (scale < std::numeric_limits<double>::epsilon())
   {
@@ -57,7 +57,7 @@ static inline bool validateScale(double scale)
   return true;
 }
 
-static inline bool validatePadding(double padding)
+static inline bool validatePadding(const double padding)
 {
   if (padding < 0.0)
   {
@@ -83,7 +83,7 @@ CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double
     padding = 0.0;
 
   const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
-  for (auto link : links)
+  for (const auto link : links)
   {
     link_padding_[link->getName()] = padding;
     link_scale_[link->getName()] = scale;
@@ -100,7 +100,7 @@ CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, const
     padding = 0.0;
 
   const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
-  for (auto link : links)
+  for (const auto link : links)
   {
     link_padding_[link->getName()] = padding;
     link_scale_[link->getName()] = scale;
@@ -113,13 +113,13 @@ CollisionEnv::CollisionEnv(const CollisionEnv& other, const WorldPtr& world)
   link_padding_ = other.link_padding_;
   link_scale_ = other.link_scale_;
 }
-void CollisionEnv::setPadding(double padding)
+void CollisionEnv::setPadding(const double padding)
 {
   if (!validatePadding(padding))
     return;
   std::vector<std::string> u;
   const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
-  for (auto link : links)
+  for (const auto link : links)
   {
     if (getLinkPadding(link->getName()) != padding)
       u.push_back(link->getName());
@@ -129,13 +129,13 @@ void CollisionEnv::setPadding(double padding)
     updatedPaddingOrScaling(u);
 }
 
-void CollisionEnv::setScale(double scale)
+void CollisionEnv::setScale(const double scale)
 {
   if (!validateScale(scale))
     return;
   std::vector<std::string> u;
   const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
-  for (auto link : links)
+  for (const auto link : links)
   {
     if (getLinkScale(link->getName()) != scale)
       u.push_back(link->getName());
@@ -145,10 +145,10 @@ void CollisionEnv::setScale(double scale)
     updatedPaddingOrScaling(u);
 }
 
-void CollisionEnv::setLinkPadding(const std::string& link_name, double padding)
+void CollisionEnv::setLinkPadding(const std::string& link_name, const double padding)
 {
   validatePadding(padding);
-  bool update = getLinkPadding(link_name) != padding;
+  const bool update = getLinkPadding(link_name) != padding;
   link_padding_[link_name] = padding;
   if (update)
   {
@@ -172,7 +172,7 @@ void CollisionEnv::setLinkPadding(const std::map<std::string, double>& padding)
   for (const auto& link_pad_pair : padding)
   {
     validatePadding(link_pad_pair.second);
-    bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
+    const bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
     link_padding_[link_pad_pair.first] = link_pad_pair.second;
     if (update)
       u.push_back(link_pad_pair.first);
@@ -186,10 +186,10 @@ const std::map<std::string, double>& CollisionEnv::getLinkPadding() const
   return link_padding_;
 }
 
-void CollisionEnv::setLinkScale(const std::string& link_name, double scale)
+void CollisionEnv::setLinkScale(const std::string& link_name, const double scale)
 {
   validateScale(scale);
-  bool update = getLinkScale(link_name) != scale;
+  const bool update = getLinkScale(link_name) != scale;
   link_scale_[link_name] = scale;
   if (update)
   {
@@ -200,7 +200,7 @@ void CollisionEnv::setLinkScale(const std::string& link_name, double scale)
 
 double CollisionEnv::getLinkScale(const std::string& link_name) const
 {
-  auto it = link_scale_.find(link_name);
+  const auto it = link_scale_.find(link_name);
   if (it != link_scale_.end())
     return it->second;
   else
@@ -212,7 +212,7 @@ void CollisionEnv::setLinkScale(const std::map<std::string, double>& scale)
   std::vector<std::string> u;
   for (const auto& link_scale_pair : scale)
   {
-    bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
+    const bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
     link_scale_[link_scale_pair.first] = link_scale_pair.second;
     if (update)
       u.push_back(link_scale_pair.first);
@@ -232,7 +232,7 @@ void CollisionEnv::setPadding(const std::vector<moveit_msgs::msg::LinkPadding>&
   for (const auto& p : padding)
   {
     validatePadding(p.padding);
-    bool update = getLinkPadding(p.link_name) != p.padding;
+    const bool update = getLinkPadding(p.link_name) != p.padding;
     link_padding_[p.link_name] = p.padding;
     if (update)
       u.push_back(p.link_name);
@@ -247,7 +247,7 @@ void CollisionEnv::setScale(const std::vector<moveit_msgs::msg::LinkScale>& scal
   for (const auto& s : scale)
   {
     validateScale(s.scale);
-    bool update = getLinkScale(s.link_name) != s.scale;
+    const bool update = getLinkScale(s.link_name) != s.scale;
     link_scale_[s.link_name] = s.scale;
     if (update)
       u.push_back(s.link_name);
diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index 8de60b1f96..e52fc6005f 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -49,7 +49,7 @@ AllowedCollisionMatrix::AllowedCollisionMatrix()
 {
 }
 
-AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed)
+AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, const bool allowed)
 {
   for (std::size_t i = 0; i < names.size(); ++i)
     for (std::size_t j = i; j < names.size(); ++j)
@@ -105,10 +105,10 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo
 
 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const
 {
-  auto it1 = allowed_contacts_.find(name1);
+  const auto it1 = allowed_contacts_.find(name1);
   if (it1 == allowed_contacts_.end())
     return false;
-  auto it2 = it1->second.find(name2);
+  const auto it2 = it1->second.find(name2);
   if (it2 == it1->second.end())
     return false;
   fn = it2->second;
@@ -118,7 +118,7 @@ bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::strin
 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2,
                                       AllowedCollision::Type& allowed_collision) const
 {
-  auto it1 = entries_.find(name1);
+  const auto it1 = entries_.find(name1);
   if (it1 == entries_.end())
     return false;
   auto it2 = it1->second.find(name2);
@@ -135,14 +135,14 @@ bool AllowedCollisionMatrix::hasEntry(const std::string& name) const
 
 bool AllowedCollisionMatrix::hasEntry(const std::string& name1, const std::string& name2) const
 {
-  auto it1 = entries_.find(name1);
+  const auto it1 = entries_.find(name1);
   if (it1 == entries_.end())
     return false;
-  auto it2 = it1->second.find(name2);
+  const auto it2 = it1->second.find(name2);
   return it2 != it1->second.end();
 }
 
-void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, bool allowed)
+void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, const bool allowed)
 {
   const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
   entries_[name1][name2] = entries_[name2][name1] = v;
@@ -213,7 +213,8 @@ void AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::st
   }
 }
 
-void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed)
+void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names,
+                                      const bool allowed)
 {
   for (const auto& other_name : other_names)
     if (other_name != name)
@@ -221,13 +222,13 @@ void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector
 }
 
 void AllowedCollisionMatrix::setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2,
-                                      bool allowed)
+                                      const bool allowed)
 {
   for (const auto& name1 : names1)
     setEntry(name1, names2, allowed);
 }
 
-void AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
+void AllowedCollisionMatrix::setEntry(const std::string& name, const bool allowed)
 {
   std::string last = name;
   for (auto& entry : entries_)
@@ -238,7 +239,7 @@ void AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
     }
 }
 
-void AllowedCollisionMatrix::setEntry(bool allowed)
+void AllowedCollisionMatrix::setEntry(const bool allowed)
 {
   const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
   for (auto& entry : entries_)
@@ -246,7 +247,7 @@ void AllowedCollisionMatrix::setEntry(bool allowed)
       it2.second = v;
 }
 
-void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, bool allowed)
+void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, const bool allowed)
 {
   const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
   default_entries_[name] = v;
@@ -285,12 +286,12 @@ static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f
 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
                                                  DecideContactFn& fn) const
 {
-  bool found = getEntry(name1, name2, fn);
+  const bool found = getEntry(name1, name2, fn);
   if (!found)
   {
     DecideContactFn fn1, fn2;
-    bool found1 = getDefaultEntry(name1, fn1);
-    bool found2 = getDefaultEntry(name2, fn2);
+    const bool found1 = getDefaultEntry(name1, fn1);
+    const bool found2 = getDefaultEntry(name2, fn2);
     if (found1 && !found2)
       fn = fn1;
     else if (!found1 && found2)
@@ -307,8 +308,8 @@ bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name1, const std
                                              AllowedCollision::Type& allowed_collision) const
 {
   AllowedCollision::Type t1, t2;
-  bool found1 = getDefaultEntry(name1, t1);
-  bool found2 = getDefaultEntry(name2, t2);
+  const bool found1 = getDefaultEntry(name1, t1);
+  const bool found2 = getDefaultEntry(name2, t2);
   if (!found1 && !found2)
     return false;
   else if (found1 && !found2)
@@ -373,7 +374,7 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix
   for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
   {
     AllowedCollision::Type dtype;
-    bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
+    const bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
     if (dfound)
     {
       msg.default_entry_names.push_back(msg.entry_names[i]);
@@ -395,9 +396,9 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
   getAllEntryNames(names);
 
   std::size_t spacing = 4;
-  for (auto& name : names)
+  for (const auto& name : names)
   {
-    std::size_t length = name.length();
+    const std::size_t length = name.length();
     if (length > spacing)
       spacing = length;
   }
@@ -436,7 +437,7 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
     // print pairs
     for (std::size_t j = 0; j < names.size(); ++j)
     {
-      bool found = getAllowedCollision(names[i], names[j], type);
+      const bool found = getAllowedCollision(names[i], names[j], type);
       if (found)
         out << std::setw(3) << indicator[type];
       else
diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp
index be5b6b955a..64bf51f758 100644
--- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp
+++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp
@@ -60,8 +60,9 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
                  const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient);
 
 int collision_detection::refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res,
-                                              double cell_bbx_search_distance, double allowed_angle_divergence,
-                                              bool estimate_depth, double iso_value, double metaball_radius_multiple)
+                                              const double cell_bbx_search_distance,
+                                              const double allowed_angle_divergence, const bool estimate_depth,
+                                              const double iso_value, const double metaball_radius_multiple)
 {
   if (!object)
   {
@@ -79,8 +80,8 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
   // iterate through contacts
   for (auto& contact : res.contacts)
   {
-    std::string contact1 = contact.first.first;
-    std::string contact2 = contact.first.second;
+    const std::string contact1 = contact.first.first;
+    const std::string contact2 = contact.first.second;
     std::string octomap_name = "";
     std::vector<collision_detection::Contact>& contact_vector = contact.second;
 
@@ -97,7 +98,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
     if (!object->shapes_.empty())
     {
       const shapes::ShapeConstPtr& shape = object->shapes_[0];
-      std::shared_ptr<const shapes::OcTree> shape_octree = std::dynamic_pointer_cast<const shapes::OcTree>(shape);
+      const std::shared_ptr<const shapes::OcTree> shape_octree = std::dynamic_pointer_cast<const shapes::OcTree>(shape);
       if (shape_octree)
       {
         std::shared_ptr<const octomap::OcTree> octree = shape_octree->octree;
@@ -107,11 +108,11 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
           const Eigen::Vector3d& point = contact_info.pos;
           const Eigen::Vector3d& normal = contact_info.normal;
 
-          octomath::Vector3 contact_point(point[0], point[1], point[2]);
-          octomath::Vector3 contact_normal(normal[0], normal[1], normal[2]);
-          octomath::Vector3 diagonal = octomath::Vector3(1, 1, 1);
-          octomath::Vector3 bbx_min = contact_point - diagonal * cell_size * cell_bbx_search_distance;
-          octomath::Vector3 bbx_max = contact_point + diagonal * cell_size * cell_bbx_search_distance;
+          const octomath::Vector3 contact_point(point[0], point[1], point[2]);
+          const octomath::Vector3 contact_normal(normal[0], normal[1], normal[2]);
+          const octomath::Vector3 diagonal = octomath::Vector3(1, 1, 1);
+          const octomath::Vector3 bbx_min = contact_point - diagonal * cell_size * cell_bbx_search_distance;
+          const octomath::Vector3 bbx_max = contact_point + diagonal * cell_size * cell_bbx_search_distance;
           octomap::point3d_list node_centers;
           octomap::OcTreeBaseImpl<octomap::OcTreeNode, octomap::AbstractOccupancyOcTree>::leaf_bbx_iterator it =
               octree->begin_leafs_bbx(bbx_min, bbx_max);
@@ -120,7 +121,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
           int count = 0;
           for (; it != leafs_end; ++it)
           {
-            octomap::point3d pt = it.getCoordinate();
+            const octomap::point3d pt = it.getCoordinate();
             // double prob = it->getOccupancy();
             if (octree->isNodeOccupied(*it))  // magic number!
             {
@@ -144,7 +145,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
                                            n, depth, estimate_depth))
           {
             // only modify normal if the refinement predicts a "very different" result.
-            double divergence = contact_normal.angleTo(n);
+            const double divergence = contact_normal.angleTo(n);
             if (divergence > allowed_angle_divergence)
             {
               modified++;
@@ -167,7 +168,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
 
 bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
                                   const double& r_multiple, const octomath::Vector3& contact_point,
-                                  octomath::Vector3& normal, double& depth, bool estimate_depth)
+                                  octomath::Vector3& normal, double& depth, const bool estimate_depth)
 {
   double intensity;
   if (estimate_depth)
@@ -215,7 +216,7 @@ bool findSurface(const octomap::point3d_list& cloud, const double& spacing, cons
   {
     if (!sampleCloud(cloud, spacing, r_multiple, p, intensity, gs))
       return false;
-    double s = iso_value - intensity;
+    const double s = iso_value - intensity;
     dp = (gs * -s) * (1.0 / std::max(gs.dot(gs), epsilon));
     p = p + dp;
     if (dp.dot(dp) < epsilon)
@@ -235,10 +236,10 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
   intensity = 0.f;
   gradient = octomath::Vector3(0, 0, 0);
 
-  double r = r_multiple * spacing;  // TODO magic number!
+  const double r = r_multiple * spacing;  // TODO magic number!
   // double T = 0.5; // TODO magic number!
 
-  int nn = cloud.size();
+  const int nn = cloud.size();
   if (nn == 0)
   {
     return false;
@@ -246,7 +247,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
 
   // variables for Wyvill
   double a = 0, b = 0, c = 0, r2 = 0, r4 = 0, r6 = 0, a1 = 0, b1 = 0, c1 = 0, a2 = 0, b2 = 0, c2 = 0;
-  bool wyvill = true;
+  const bool wyvill = true;
 
   for (const octomath::Vector3& v : cloud)
   {
@@ -274,17 +275,17 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
     octomath::Vector3 f_grad(0, 0, 0);
 
     octomath::Vector3 pos = position - v;
-    double r = pos.norm();
+    const double r = pos.norm();
     pos = pos * (1.0 / r);
     if (r > r)  // must skip points outside valid bounds.
     {
       continue;
     }
-    double r2 = r * r;
-    double r3 = r * r2;
-    double r4 = r2 * r2;
-    double r5 = r3 * r2;
-    double r6 = r3 * r3;
+    const double r2 = r * r;
+    const double r3 = r * r2;
+    const double r4 = r2 * r2;
+    const double r5 = r3 * r2;
+    const double r6 = r3 * r3;
 
     if (wyvill)
     {
@@ -294,7 +295,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
     else
     {
       RCLCPP_ERROR(LOGGER, "This should not be called!");
-      double r_scaled = r / r;
+      const double r_scaled = r / r;
       // TODO still need to address the scaling...
       f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1);
       f_grad = pos * (-4.0 / r * pow(1.0 - r_scaled, 3) * (4.0 * r_scaled + 1.0) + 4.0 / r * pow(1 - r_scaled, 4));
diff --git a/moveit_core/collision_detection/src/collision_plugin_cache.cpp b/moveit_core/collision_detection/src/collision_plugin_cache.cpp
index 9ad1ffd09f..ff36bf04a6 100644
--- a/moveit_core/collision_detection/src/collision_plugin_cache.cpp
+++ b/moveit_core/collision_detection/src/collision_plugin_cache.cpp
@@ -78,7 +78,7 @@ class CollisionPluginCache::CollisionPluginCacheImpl
     std::map<std::string, CollisionPluginPtr>::iterator it = plugins_.find(name);
     if (it == plugins_.end())
     {
-      CollisionPluginPtr plugin = load(name);
+      const CollisionPluginPtr plugin = load(name);
       if (plugin)
       {
         return plugin->initialize(scene);
diff --git a/moveit_core/collision_detection/src/collision_tools.cpp b/moveit_core/collision_detection/src/collision_tools.cpp
index 6f6d57610f..fd33a6b2c2 100644
--- a/moveit_core/collision_detection/src/collision_tools.cpp
+++ b/moveit_core/collision_detection/src/collision_tools.cpp
@@ -184,12 +184,12 @@ void intersectCostSources(std::set<CostSource>& cost_sources, const std::set<Cos
     }
 }
 
-void removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fraction)
+void removeOverlapping(std::set<CostSource>& cost_sources, const double overlap_fraction)
 {
   double p[3], q[3];
   for (auto it = cost_sources.begin(); it != cost_sources.end(); ++it)
   {
-    double vol = it->getVolume() * overlap_fraction;
+    const double vol = it->getVolume() * overlap_fraction;
     std::vector<std::set<CostSource>::iterator> remove;
     auto it1 = it;
     for (auto jt = ++it1; jt != cost_sources.end(); ++jt)
@@ -205,7 +205,7 @@ void removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fracti
       if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
         continue;
 
-      double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
+      const double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
       if (intersect_volume >= vol)
         remove.push_back(jt);
     }
@@ -215,7 +215,7 @@ void removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fracti
 }
 
 void removeCostSources(std::set<CostSource>& cost_sources, const std::set<CostSource>& cost_sources_to_remove,
-                       double overlap_fraction)
+                       const double overlap_fraction)
 {
   // remove all the boxes that overlap with the intersection previously computed in \e rem
   double p[3], q[3];
@@ -236,7 +236,7 @@ void removeCostSources(std::set<CostSource>& cost_sources, const std::set<CostSo
       if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
         continue;
 
-      double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
+      const double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
       if (intersect_volume >= it->getVolume() * overlap_fraction)
         remove.push_back(it);
       else
diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp
index 713b2997ab..ae2cb441b1 100644
--- a/moveit_core/collision_detection/src/world.cpp
+++ b/moveit_core/collision_detection/src/world.cpp
@@ -110,7 +110,7 @@ std::vector<std::string> World::getObjectIds() const
 
 World::ObjectConstPtr World::getObject(const std::string& object_id) const
 {
-  auto it = objects_.find(object_id);
+  const auto it = objects_.find(object_id);
   if (it == objects_.end())
     return ObjectConstPtr();
   else
@@ -131,7 +131,7 @@ bool World::hasObject(const std::string& object_id) const
 bool World::knowsTransform(const std::string& name) const
 {
   // Check object names first
-  std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
+  const std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
   if (it != objects_.end())
     return true;
   else  // Then objects' subframes
@@ -164,7 +164,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram
   // assume found
   frame_found = true;
 
-  std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
+  const std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
   if (it != objects_.end())
   {
     return it->second->pose_;
@@ -177,7 +177,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram
       // rfind searches name for object.first in the first index (returns 0 if found)
       if (name.rfind(object.first, 0) == 0 && name[object.first.length()] == '/')
       {
-        auto it = object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1));
+        const auto it = object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1));
         if (it != object.second->global_subframe_poses_.end())
         {
           return it->second;
@@ -192,9 +192,9 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram
   return IDENTITY_TRANSFORM;
 }
 
-const Eigen::Isometry3d& World::getGlobalShapeTransform(const std::string& object_id, int shape_index) const
+const Eigen::Isometry3d& World::getGlobalShapeTransform(const std::string& object_id, const int shape_index) const
 {
-  auto it = objects_.find(object_id);
+  const auto it = objects_.find(object_id);
   if (it != objects_.end())
   {
     return it->second->global_shape_poses_[shape_index];
@@ -209,7 +209,7 @@ const Eigen::Isometry3d& World::getGlobalShapeTransform(const std::string& objec
 
 const EigenSTL::vector_Isometry3d& World::getGlobalShapeTransforms(const std::string& object_id) const
 {
-  auto it = objects_.find(object_id);
+  const auto it = objects_.find(object_id);
   if (it != objects_.end())
   {
     return it->second->global_shape_poses_;
@@ -225,10 +225,10 @@ const EigenSTL::vector_Isometry3d& World::getGlobalShapeTransforms(const std::st
 bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape,
                               const Eigen::Isometry3d& shape_pose)
 {
-  auto it = objects_.find(object_id);
+  const auto it = objects_.find(object_id);
   if (it != objects_.end())
   {
-    unsigned int n = it->second->shapes_.size();
+    const unsigned int n = it->second->shapes_.size();
     for (unsigned int i = 0; i < n; ++i)
       if (it->second->shapes_[i] == shape)
       {
@@ -246,7 +246,7 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC
 
 bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform)
 {
-  auto it = objects_.find(object_id);
+  const auto it = objects_.find(object_id);
   if (it == objects_.end())
     return false;
   if (transform.isApprox(Eigen::Isometry3d::Identity()))
@@ -280,10 +280,10 @@ bool World::setObjectPose(const std::string& object_id, const Eigen::Isometry3d&
 
 bool World::removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape)
 {
-  auto it = objects_.find(object_id);
+  const auto it = objects_.find(object_id);
   if (it != objects_.end())
   {
-    unsigned int n = it->second->shapes_.size();
+    const unsigned int n = it->second->shapes_.size();
     for (unsigned int i = 0; i < n; ++i)
       if (it->second->shapes_[i] == shape)
       {
@@ -309,7 +309,7 @@ bool World::removeShapeFromObject(const std::string& object_id, const shapes::Sh
 
 bool World::removeObject(const std::string& object_id)
 {
-  auto it = objects_.find(object_id);
+  const auto it = objects_.find(object_id);
   if (it != objects_.end())
   {
     notify(it->second, DESTROY);
@@ -327,7 +327,7 @@ void World::clearObjects()
 
 bool World::setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses)
 {
-  auto obj_pair = objects_.find(object_id);
+  const auto obj_pair = objects_.find(object_id);
   if (obj_pair == objects_.end())
   {
     return false;
@@ -363,7 +363,7 @@ void World::updateGlobalPosesInternal(ObjectPtr& obj, bool update_shape_poses, b
 
 World::ObserverHandle World::addObserver(const ObserverCallbackFn& callback)
 {
-  auto o = new Observer(callback);
+  const auto o = new Observer(callback);
   observers_.push_back(o);
   return ObserverHandle(o);
 }