Wnon-virtual-dtor and Wold-style-cast (#3)

CMakeLists.txt

@@ -47,6 +47,7 @@ find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(backward_ros REQUIRED)
 
 # TODO(tylerjw): re-enable these
 # find_package(OpenMP)

cmake/CompilerWarnings.cmake

@@ -66,8 +66,8 @@ function(set_project_warnings project_name)
       -Wshadow # warn the user if a variable declaration shadows one from a parent context
       # warn the user if a class with virtual functions has a non-virtual destructor. This helps
       # catch hard to track down memory errors
-      # -Wnon-virtual-dtor
-      # -Wold-style-cast # warn for c-style casts
+      -Wnon-virtual-dtor
+      -Wold-style-cast # warn for c-style casts
       # -Wcast-align # warn for potential performance problem casts
       # -Wunused # warn on anything being unused
       # -Woverloaded-virtual # warn if you overload (not override) a virtual function

package.xml

@@ -21,6 +21,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>tf2_kdl</depend>
+  <depend>backward_ros</depend>
 
   <!-- <depend>tf2</depend> -->
   <!-- <depend>tf2_eigen</depend> -->

src/forward_kinematics.hpp

@@ -909,26 +909,27 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
       if (mutation_approx_mask[itip][variable_index] == 0) continue;
       auto& joint_delta = mutation_approx_frames[itip][variable_index];
       const Frame& tip_frame = input[itip];
-      const double* tip_frame_ptr = (const double*)&tip_frame;
+      const double* tip_frame_ptr = reinterpret_cast<const double*>(&tip_frame);
       __m256d p = _mm256_load_pd(tip_frame_ptr + 0);
       __m256d r = _mm256_load_pd(tip_frame_ptr + 4);
       {
-        auto joint_delta_ptr = (const double* __restrict__) & (joint_delta);
+        auto joint_delta_ptr =
+            reinterpret_cast<const double* __restrict__>(&joint_delta);
         __m256d ff = _mm256_set1_pd(variable_delta);
         p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p);
         r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r);
       }
 #if USE_QUADRATIC_EXTRAPOLATION
       {
-        auto joint_delta_ptr =
-            (const double* __restrict__) &
-            (mutation_approx_quadratics[itip][variable_index]);
+        auto joint_delta_ptr = static_cast<const double* __restrict__>(
+            &mutation_approx_quadratics[itip][variable_index]);
         __m256d ff = _mm256_set1_pd(variable_delta * variable_delta);
         p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p);
       }
 #endif
       auto& tip_mutation = output[itip];
-      double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation;
+      double* __restrict__ tip_mutation_ptr =
+          reinterpret_cast<double*>(&tip_mutation);
       _mm256_store_pd(tip_mutation_ptr + 0, p);
       _mm256_store_pd(tip_mutation_ptr + 4, r);
     }
@@ -946,13 +947,14 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
       if (mutation_approx_mask[itip][variable_index] == 0) continue;
       auto& joint_delta = mutation_approx_frames[itip][variable_index];
       const Frame& tip_frame = input[itip];
-      const double* tip_frame_ptr = (const double*)&tip_frame;
+      const double* tip_frame_ptr = reinterpret_cast<const double*>(&tip_frame);
       __m128d pxy = _mm_load_pd(tip_frame_ptr + 0);
       __m128d pzw = _mm_load_sd(tip_frame_ptr + 2);
       __m128d rxy = _mm_load_pd(tip_frame_ptr + 4);
       __m128d rzw = _mm_load_pd(tip_frame_ptr + 6);
       {
-        auto joint_delta_ptr = (const double* __restrict__) & (joint_delta);
+        auto joint_delta_ptr =
+            reinterpret_cast<const double* __restrict__>(&joint_delta);
         __m128d ff = _mm_set1_pd(variable_delta);
         pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)));
         pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)));
@@ -961,16 +963,16 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
       }
 #if USE_QUADRATIC_EXTRAPOLATION
       {
-        auto joint_delta_ptr =
-            (const double* __restrict__) &
-            (mutation_approx_quadratics[itip][variable_index]);
+        auto joint_delta_ptr = static_cast<const double* __restrict__>(
+            &mutation_approx_quadratics[itip][variable_index]);
         __m128d ff = _mm_set1_pd(variable_delta * variable_delta);
         pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)));
         pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)));
       }
 #endif
       auto& tip_mutation = output[itip];
-      double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation;
+      double* __restrict__ tip_mutation_ptr =
+          reinterpret_cast<double*>(&tip_mutation);
       _mm_store_pd(tip_mutation_ptr + 0, pxy);
       _mm_store_sd(tip_mutation_ptr + 2, pzw);
       _mm_store_pd(tip_mutation_ptr + 4, rxy);
@@ -1051,7 +1053,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
 
       const Frame& tip_frame = tip_frames_aligned[itip];
 
-      const double* tip_frame_ptr = (const double*)&tip_frame;
+      const double* tip_frame_ptr = reinterpret_cast<const double*>(&tip_frame);
       __m256d p0 = _mm256_load_pd(tip_frame_ptr + 0);
       __m256d r0 = _mm256_load_pd(tip_frame_ptr + 4);
 
@@ -1065,26 +1067,26 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
               mutation_values[imutation][vii] - p_variables[variable_index];
 
           {
-            auto joint_delta_ptr =
-                (const double* __restrict__) & (joint_deltas[variable_index]);
+            auto joint_delta_ptr = reinterpret_cast<const double* __restrict__>(
+                &joint_deltas[variable_index]);
             __m256d ff = _mm256_set1_pd(variable_delta);
             p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p);
             r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r);
           }
 
 #if USE_QUADRATIC_EXTRAPOLATION
           {
-            auto joint_delta_ptr =
-                (const double* __restrict__) &
-                (mutation_approx_quadratics[itip][variable_index]);
+            auto joint_delta_ptr = reinterpret_cast<const double* __restrict__>(
+                &mutation_approx_quadratics[itip][variable_index]);
             __m256d ff = _mm256_set1_pd(variable_delta * variable_delta);
             p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p);
           }
 #endif
         }
 
         auto& tip_mutation = tip_frame_mutations[imutation][itip];
-        double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation;
+        double* __restrict__ tip_mutation_ptr =
+            reinterpret_cast<double*>(&tip_mutation);
         _mm256_store_pd(tip_mutation_ptr + 0, p);
         _mm256_store_pd(tip_mutation_ptr + 4, r);
       }
@@ -1105,7 +1107,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
 
       const Frame& tip_frame = tip_frames_aligned[itip];
 
-      const double* tip_frame_ptr = (const double*)&tip_frame;
+      const double* tip_frame_ptr = reinterpret_cast<const double*>(&tip_frame);
       __m128d pxy0 = _mm_load_pd(tip_frame_ptr + 0);
       __m128d pzw0 = _mm_load_sd(tip_frame_ptr + 2);
       __m128d rxy0 = _mm_load_pd(tip_frame_ptr + 4);
@@ -1123,8 +1125,8 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
               mutation_values[imutation][vii] - p_variables[variable_index];
 
           {
-            auto joint_delta_ptr =
-                (const double* __restrict__) & (joint_deltas[variable_index]);
+            auto joint_delta_ptr = reinterpret_cast<const double* __restrict__>(
+                &joint_deltas[variable_index]);
             __m128d ff = _mm_set1_pd(variable_delta);
             pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)),
                              pxy);
@@ -1138,9 +1140,8 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
 
 #if USE_QUADRATIC_EXTRAPOLATION
           {
-            auto joint_delta_ptr =
-                (const double* __restrict__) &
-                (mutation_approx_quadratics[itip][variable_index]);
+            auto joint_delta_ptr = reinterpret_cast<const double* __restrict__>(
+                &mutation_approx_quadratics[itip][variable_index]);
             __m128d ff = _mm_set1_pd(variable_delta * variable_delta);
             pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)),
                              pxy);
@@ -1151,7 +1152,8 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
         }
 
         auto& tip_mutation = tip_frame_mutations[imutation][itip];
-        double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation;
+        double* __restrict__ tip_mutation_ptr =
+            reinterpret_cast<double*>(&tip_mutation);
         _mm_store_pd(tip_mutation_ptr + 0, pxy);
         _mm_store_sd(tip_mutation_ptr + 2, pzw);
         _mm_store_pd(tip_mutation_ptr + 4, rxy);

src/goal_types.cpp

@@ -135,11 +135,14 @@ void TouchGoal::describe(GoalContext& context) const {
         // s.points.size(), s.polygons.data());
 
         // workaround for fcl::Convex initialization bug
-        auto* fcl = (fcl::Convex*)::operator new(sizeof(fcl::Convex));
-        fcl->num_points = s.points.size();
-        fcl = new (fcl) fcl::Convex(s.plane_normals.data(), s.plane_dis.data(),
-                                    s.plane_normals.size(), s.points.data(),
-                                    s.points.size(), s.polygons.data());
+        auto fcl = [&s]() -> fcl::Convex* {
+          auto buffer = operator new(sizeof(fcl::Convex));
+          static_cast<fcl::Convex*>(buffer)->num_points = s.points.size();
+          return static_cast<fcl::Convex*>(new (buffer) fcl::Convex(
+              s.plane_normals.data(), s.plane_dis.data(),
+              s.plane_normals.size(), s.points.data(), s.points.size(),
+              s.polygons.data()));
+        }();
 
         s.geometry = decltype(s.geometry)(
             new collision_detection::FCLGeometry(fcl, link_model, shape_index));

src/ik_evolution_2.cpp

@@ -259,7 +259,8 @@ struct IKEvolution2 : IKBase {
                children_.size() * 4 + 4;
 
     auto* __restrict__ rr = fast_random_gauss_n(s);
-    rr = (const double*)(((size_t)rr + 3) / 4 * 4);
+    rr = reinterpret_cast<const double*>((reinterpret_cast<size_t>(rr) + 3) /
+                                         4 * 4);
 
     /*rmask.resize(s);
     for(auto& m : rmask) m = fast_random() < 0.1 ? 1.0 : 0.0;
@@ -344,8 +345,8 @@ struct IKEvolution2 : IKBase {
       }*/
 
       for (auto quaternion_gene_index : quaternion_genes_) {
-        auto& qpos = (*(Quaternion*)&(
-            children_[child_index].genes[quaternion_gene_index]));
+        auto& qpos = (*reinterpret_cast<Quaternion*>(
+            &children_[child_index].genes[quaternion_gene_index]));
         normalizeFast(qpos);
       }
     }

src/kinematics_plugin.cpp

@@ -88,7 +88,7 @@ bool isBioIKKinematicsQueryOptions(const void *ptr) {
 const BioIKKinematicsQueryOptions *toBioIKKinematicsQueryOptions(
     const void *ptr) {
   if (isBioIKKinematicsQueryOptions(ptr))
-    return (const BioIKKinematicsQueryOptions *)ptr;
+    return static_cast<const BioIKKinematicsQueryOptions *>(ptr);
   else
     return 0;
 }
@@ -462,7 +462,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
     {
       if (!bio_ik_options || !bio_ik_options->replace) {
         for (size_t i = 0; i < tip_frames_.size(); i++) {
-          auto *goal = (PoseGoal *)default_goals[i].get();
+          auto *goal = static_cast<PoseGoal *>(default_goals[i].get());
           goal->setPosition(tipFrames[i].pos);
           goal->setOrientation(tipFrames[i].rot);
         }

src/problem.cpp

@@ -77,10 +77,10 @@ void Problem::initialize(moveit::core::RobotModelConstPtr robot_model,
                          const std::vector<const Goal*>& goals2,
                          const BioIKKinematicsQueryOptions* options) {
   if (robot_model != robot_model_) {
-    modelInfo_ = RobotInfo(robot_model_);
+    modelInfo_ = RobotInfo(robot_model);
 #if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0))
     collision_links_.clear();
-    collision_links_.resize(robot_model_->getLinkModelCount());
+    collision_links_.resize(robot_model->getLinkModelCount());
 #endif
   }
 
@@ -112,7 +112,8 @@ void Problem::initialize(moveit::core::RobotModelConstPtr robot_model,
       auto& joint_name = robot_model_->getJointOfVariable(name)->getName();
       for (auto& fixed_joint_name : options->fixed_joints) {
         if (fixed_joint_name == joint_name) {
-          return (ssize_t)-1 - (ssize_t)robot_model_->getVariableIndex(name);
+          return static_cast<ssize_t>(-1) -
+                 static_cast<ssize_t>(robot_model_->getVariableIndex(name));
         }
       }
     }

src/utils.hpp

@@ -380,6 +380,7 @@ class Factory {
     virtual BASE* create(ARGS... args) const = 0;
     virtual BASE* clone(const BASE*) const = 0;
     ClassBase() : type_(typeid(void)) {}
+    virtual ~ClassBase() {}
   };
   typedef std::set<ClassBase*> MapType;
   static MapType& classes() {
@@ -391,13 +392,15 @@ class Factory {
   template <class DERIVED>
   struct Class : ClassBase {
     BASE* create(ARGS... args) const { return new DERIVED(args...); }
-    BASE* clone(const BASE* o) const { return new DERIVED(*(const DERIVED*)o); }
+    BASE* clone(const BASE* o) const {
+      return new DERIVED(*static_cast<const DERIVED*>(o));
+    }
     Class(const std::string& name) {
       ClassBase::name_ = name;
       ClassBase::type_ = typeid(DERIVED);
       classes().insert(this);
     }
-    ~Class() { classes().erase(this); }
+    virtual ~Class() { classes().erase(this); }
   };
   static BASE* create(const std::string& name, ARGS... args) {
     for (auto* f : classes())
@@ -407,7 +410,7 @@ class Factory {
   template <class DERIVED>
   static DERIVED* clone(const DERIVED* o) {
     for (auto* f : classes())
-      if (f->type_ == typeid(*o)) return (DERIVED*)f->clone(o);
+      if (f->type_ == typeid(*o)) return static_cast<DERIVED*>(f->clone(o));
     ERROR("class not found", typeid(*o).name());
   }
 };
@@ -425,7 +428,7 @@ struct aligned_allocator : public std::allocator<T> {
   T* allocate(size_t s, [[maybe_unused]] const void* hint = 0) {
     void* p;
     if (posix_memalign(&p, A, sizeof(T) * s + 64)) throw std::bad_alloc();
-    return (T*)p;
+    return static_cast<T*>(p);
   }
   void deallocate(T* ptr, [[maybe_unused]] size_t s) { free(ptr); }
   template <class U>