Enabling and fixing more compiler warnings (#4)

Signed-off-by: Tyler Weaver <[email protected]>

.github/workflows/ci.yaml

@@ -17,13 +17,14 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - NAME: "rolling-testing-ccov"
-            ROS_REPO: testing
+          # TODO(tylerjw): re-enable testing on testing once moveit is fixed on buildfarm
+          # - NAME: "rolling-testing-ccov"
+          #   ROS_REPO: testing
             # TODO(tylerjw): enable coverage once there is a test
             # TARGET_CMAKE_ARGS: -DENABLE_COVERAGE=ON
             # CCOV: true
           - NAME: "rolling-main"
-            ROS_REPO: main
+            # ROS_REPO: main
           - NAME: "address-leak-ub-sanitizers"
             TARGET_CMAKE_ARGS: >
               -DCMAKE_BUILD_TYPE=Debug
@@ -52,6 +53,10 @@ jobs:
 
     env:
       ROS_DISTRO: rolling
+      # TODO(tylerjw): re-enable testing on testing once moveit is fixed on buildfarm
+      ROS_REPO: main
+      UPSTREAM_WORKSPACE: upstream.repos
+      UPSTREAM_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release
       # Clear the ccache stats before and log the stats after the build
       AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G
       AFTER_BUILD_TARGET_WORKSPACE: ccache --show-stats
@@ -62,6 +67,14 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v2
+      - name: Cache upstream workspace
+        uses: pat-s/[email protected]
+        with:
+          path: ${{ env.BASEDIR }}/upstream_ws
+          key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
+          restore-keys: ${{ env.CACHE_PREFIX }}
+        env:
+          CACHE_PREFIX: upstream_ws-${{ matrix.env.NAME }}-${{ hashFiles('upatream.repos', '.github/workflows/ci.yaml') }}
       # The target directory cache doesn't include the source directory because
       # that comes from the checkout.  See "prepare target_ws for cache" task below
       - name: Cache target workspace

CMakeLists.txt

@@ -191,7 +191,7 @@ endif()
 
 install(
   DIRECTORY include/
-  DESTINATION include
+  DESTINATION include/${PROJECT_NAME}
 )
 install(
   TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin

cmake/CompilerWarnings.cmake

@@ -68,12 +68,12 @@ function(set_project_warnings project_name)
       # catch hard to track down memory errors
       -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
-      # -Wpedantic # warn if non-standard C++ is used
-      # -Wconversion # warn on type conversions that may lose data
-      # -Wsign-conversion # warn on sign conversions
+      -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
+      -Wpedantic # warn if non-standard C++ is used
+      -Wconversion # warn on type conversions that may lose data
+      -Wsign-conversion # warn on sign conversions
       # -Wnull-dereference # warn if a null dereference is detected
       # -Wdouble-promotion # warn if float is implicit promoted to double
       # -Wformat=2 # warn on security issues around functions that format output (ie printf)

include/bio_ik/frame.hpp

@@ -43,7 +43,7 @@ typedef tf2::Vector3 Vector3;
 
 struct Frame {
   Vector3 pos;
-  double __padding[4 - (sizeof(Vector3) / sizeof(double))];
+  std::array<double, 4 - (sizeof(Vector3) / sizeof(double))> __padding;
   Quaternion rot;
   inline Frame() {}
   inline Frame(const tf2::Vector3& _pos, const tf2::Quaternion& _rot)

include/bio_ik/goal.hpp

@@ -65,7 +65,7 @@ class GoalContext {
     if (j >= 0)
       return active_variable_positions_[j];
     else
-      return initial_guess_[-1 - j];
+      return initial_guess_[static_cast<size_t>(std::labs(j))];
   }
   inline const Frame& getProblemLinkFrame(size_t i) const {
     return tip_link_frames_[i];

include/bio_ik/robot_info.hpp

@@ -67,7 +67,8 @@ class RobotInfo {
 
       bool bounded = bounds.position_bounded_;
 
-      auto* joint_model = model->getJointOfVariable(variables.size());
+      auto* joint_model =
+          model->getJointOfVariable(static_cast<int>(variables.size()));
       if (dynamic_cast<const moveit::core::RevoluteJointModel*>(joint_model)) {
         if (bounds.max_position_ - bounds.min_position_ >= 2 * M_PI * 0.9999) {
           bounded = false;
@@ -93,7 +94,7 @@ class RobotInfo {
 
     for (size_t ivar = 0; ivar < model->getVariableCount(); ivar++) {
       variable_joint_types.push_back(
-          model->getJointOfVariable(ivar)->getType());
+          model->getJointOfVariable(static_cast<int>(ivar))->getType());
     }
   }
 

src/forward_kinematics.hpp

@@ -200,7 +200,7 @@ class RobotJointEvaluator {
 
     joint_axis_list_.clear();
     joint_axis_list_.resize(robot_model_->getJointModelCount());
-    for (size_t i = 0; i < joint_axis_list_.size(); i++) {
+    for (size_t i = 0; i < joint_axis_list_.size(); ++i) {
       auto* joint_model = robot_model_->getJointModel(i);
       if (auto* j = dynamic_cast<const moveit::core::RevoluteJointModel*>(
               joint_model))
@@ -565,9 +565,12 @@ class RobotFK_Jacobian : public RobotFK_Fast {
     // double half_step_size = step_size * 0.5;
     double inv_step_size = 1.0 / step_size;
     auto tip_count = tip_frames.size();
-    jacobian.resize(tip_count * 6, variable_indices.size());
-    for (size_t icol = 0; icol < variable_indices.size(); icol++) {
-      for (size_t itip = 0; itip < tip_count; itip++) {
+    jacobian.resize(static_cast<Eigen::Index>(tip_count * 6),
+                    static_cast<Eigen::Index>(variable_indices.size()));
+    for (Eigen::Index icol = 0;
+         icol < static_cast<Eigen::Index>(variable_indices.size()); ++icol) {
+      for (Eigen::Index itip = 0; itip < static_cast<Eigen::Index>(tip_count);
+           ++itip) {
         jacobian(itip * 6 + 0, icol) = 0;
         jacobian(itip * 6 + 1, icol) = 0;
         jacobian(itip * 6 + 2, icol) = 0;
@@ -578,7 +581,8 @@ class RobotFK_Jacobian : public RobotFK_Fast {
     }
     for (size_t icol = 0; icol < variable_indices.size(); icol++) {
       auto ivar = variable_indices[icol];
-      auto* var_joint_model = robot_model_->getJointOfVariable(ivar);
+      auto* var_joint_model =
+          robot_model_->getJointOfVariable(static_cast<int>(ivar));
       if (var_joint_model->getMimic()) continue;
       for (auto* joint_model :
            joint_dependencies[var_joint_model->getJointIndex()]) {
@@ -613,13 +617,15 @@ class RobotFK_Jacobian : public RobotFK_Fast {
 
               vel = vel.cross(rot);
 
-              jacobian(itip * 6 + 0, icol) += vel.x() * scale;
-              jacobian(itip * 6 + 1, icol) += vel.y() * scale;
-              jacobian(itip * 6 + 2, icol) += vel.z() * scale;
+              auto eigen_itip = static_cast<Eigen::Index>(itip);
+              auto eigen_icol = static_cast<Eigen::Index>(icol);
+              jacobian(eigen_itip * 6 + 0, eigen_icol) += vel.x() * scale;
+              jacobian(eigen_itip * 6 + 1, eigen_icol) += vel.y() * scale;
+              jacobian(eigen_itip * 6 + 2, eigen_icol) += vel.z() * scale;
 
-              jacobian(itip * 6 + 3, icol) += rot.x() * scale;
-              jacobian(itip * 6 + 4, icol) += rot.y() * scale;
-              jacobian(itip * 6 + 5, icol) += rot.z() * scale;
+              jacobian(eigen_itip * 6 + 3, eigen_icol) += rot.x() * scale;
+              jacobian(eigen_itip * 6 + 4, eigen_icol) += rot.y() * scale;
+              jacobian(eigen_itip * 6 + 5, eigen_icol) += rot.z() * scale;
 
               // LOG("a", vel.x(), vel.y(), vel.z(), rot.x(), rot.y(), rot.z());
             }
@@ -641,9 +647,11 @@ class RobotFK_Jacobian : public RobotFK_Fast {
               auto v = axis;
               quat_mul_vec(q, axis, v);
 
-              jacobian(itip * 6 + 0, icol) += v.x() * scale;
-              jacobian(itip * 6 + 1, icol) += v.y() * scale;
-              jacobian(itip * 6 + 2, icol) += v.z() * scale;
+              auto eigen_itip = static_cast<Eigen::Index>(itip);
+              auto eigen_icol = static_cast<Eigen::Index>(icol);
+              jacobian(eigen_itip * 6 + 0, eigen_icol) += v.x() * scale;
+              jacobian(eigen_itip * 6 + 1, eigen_icol) += v.y() * scale;
+              jacobian(eigen_itip * 6 + 2, eigen_icol) += v.z() * scale;
 
               // LOG("a", v.x(), v.y(), v.z(), 0, 0, 0);
             }
@@ -677,17 +685,19 @@ class RobotFK_Jacobian : public RobotFK_Fast {
               Frame tip_frame_2;
               change(link_frame_2, link_frame_1, tip_frame_1, tip_frame_2);
               auto twist = frameTwist(tip_frame_1, tip_frame_2);
-              jacobian(itip * 6 + 0, icol) +=
+              auto eigen_itip = static_cast<Eigen::Index>(itip);
+              auto eigen_icol = static_cast<Eigen::Index>(icol);
+              jacobian(eigen_itip * 6 + 0, eigen_icol) +=
                   twist.vel.x() * inv_step_size * scale;
-              jacobian(itip * 6 + 1, icol) +=
+              jacobian(eigen_itip * 6 + 1, eigen_icol) +=
                   twist.vel.y() * inv_step_size * scale;
-              jacobian(itip * 6 + 2, icol) +=
+              jacobian(eigen_itip * 6 + 2, eigen_icol) +=
                   twist.vel.z() * inv_step_size * scale;
-              jacobian(itip * 6 + 3, icol) +=
+              jacobian(eigen_itip * 6 + 3, eigen_icol) +=
                   twist.rot.x() * inv_step_size * scale;
-              jacobian(itip * 6 + 4, icol) +=
+              jacobian(eigen_itip * 6 + 4, eigen_icol) +=
                   twist.rot.y() * inv_step_size * scale;
-              jacobian(itip * 6 + 5, icol) +=
+              jacobian(eigen_itip * 6 + 5, eigen_icol) +=
                   twist.rot.z() * inv_step_size * scale;
             }
             continue;
@@ -795,18 +805,25 @@ class RobotFK_Mutator : public RobotFK_Jacobian {
         for (size_t itip = 0; itip < tip_count; itip++) {
           {
             Vector3 t;
-            t.setX(mutation_approx_jacobian(itip * 6 + 0, icol));
-            t.setY(mutation_approx_jacobian(itip * 6 + 1, icol));
-            t.setZ(mutation_approx_jacobian(itip * 6 + 2, icol));
+            auto eigen_itip = static_cast<Eigen::Index>(itip);
+            auto eigen_icol = static_cast<Eigen::Index>(icol);
+            t.setX(mutation_approx_jacobian(eigen_itip * 6 + 0, eigen_icol));
+            t.setY(mutation_approx_jacobian(eigen_itip * 6 + 1, eigen_icol));
+            t.setZ(mutation_approx_jacobian(eigen_itip * 6 + 2, eigen_icol));
             quat_mul_vec(tip_frames[itip].rot, t, t);
             mutation_approx_frames[itip][ivar].pos = t;
           }
 
           {
             Quaternion q;
-            q.setX(mutation_approx_jacobian(itip * 6 + 3, icol) * 0.5);
-            q.setY(mutation_approx_jacobian(itip * 6 + 4, icol) * 0.5);
-            q.setZ(mutation_approx_jacobian(itip * 6 + 5, icol) * 0.5);
+            auto eigen_itip = static_cast<Eigen::Index>(itip);
+            auto eigen_icol = static_cast<Eigen::Index>(icol);
+            q.setX(mutation_approx_jacobian(eigen_itip * 6 + 3, eigen_icol) *
+                   0.5);
+            q.setY(mutation_approx_jacobian(eigen_itip * 6 + 4, eigen_icol) *
+                   0.5);
+            q.setZ(mutation_approx_jacobian(eigen_itip * 6 + 5, eigen_icol) *
+                   0.5);
             q.setW(1.0);
             quat_mul_quat(tip_frames[itip].rot, q, q);
             q -= tip_frames[itip].rot;

src/goal_types.cpp

@@ -111,9 +111,12 @@ void TouchGoal::describe(GoalContext& context) const {
             for (size_t triangle_index = 0;
                  triangle_index < triangles.size() / 3; triangle_index++) {
               polygons.push_back(3);
-              polygons.push_back(triangles[triangle_index * 3 + 0]);
-              polygons.push_back(triangles[triangle_index * 3 + 1]);
-              polygons.push_back(triangles[triangle_index * 3 + 2]);
+              polygons.push_back(
+                  static_cast<int>(triangles[triangle_index * 3 + 0]));
+              polygons.push_back(
+                  static_cast<int>(triangles[triangle_index * 3 + 1]));
+              polygons.push_back(
+                  static_cast<int>(triangles[triangle_index * 3 + 2]));
             }
             // planes are given in the same order as the triangles, though
             // redundant ones will appear only once.
@@ -137,35 +140,39 @@ void TouchGoal::describe(GoalContext& context) const {
         // workaround for fcl::Convex initialization bug
         auto fcl = [&s]() -> fcl::Convex* {
           auto buffer = operator new(sizeof(fcl::Convex));
-          static_cast<fcl::Convex*>(buffer)->num_points = s.points.size();
+          static_cast<fcl::Convex*>(buffer)->num_points =
+              static_cast<int>(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()));
+              static_cast<int>(s.plane_normals.size()), s.points.data(),
+              static_cast<int>(s.points.size()), s.polygons.data()));
         }();
 
-        s.geometry = decltype(s.geometry)(
-            new collision_detection::FCLGeometry(fcl, link_model, shape_index));
+        s.geometry = decltype(s.geometry)(new collision_detection::FCLGeometry(
+            fcl, link_model, static_cast<int>(shape_index)));
         s.edges.resize(s.points.size());
         std::vector<std::unordered_set<size_t>> edge_sets(s.points.size());
         for (size_t edge_index = 0;
              edge_index < static_cast<size_t>(fcl->num_edges); edge_index++) {
           auto edge = fcl->edges[edge_index];
-          if (edge_sets[edge.first].find(edge.second) ==
-              edge_sets[edge.first].end()) {
-            edge_sets[edge.first].insert(edge.second);
-            s.edges[edge.first].push_back(edge.second);
+          auto first_index = static_cast<size_t>(edge.first);
+          auto second_index = static_cast<size_t>(edge.second);
+          if (edge_sets[first_index].find(second_index) ==
+              edge_sets[first_index].end()) {
+            edge_sets[first_index].insert(second_index);
+            s.edges[first_index].push_back(second_index);
           }
-          if (edge_sets[edge.second].find(edge.first) ==
-              edge_sets[edge.second].end()) {
-            edge_sets[edge.second].insert(edge.first);
-            s.edges[edge.second].push_back(edge.first);
+          if (edge_sets[second_index].find(first_index) ==
+              edge_sets[second_index].end()) {
+            edge_sets[second_index].insert(first_index);
+            s.edges[second_index].push_back(first_index);
           }
         }
         for (auto& p : s.points) s.vertices.emplace_back(p[0], p[1], p[2]);
       } else {
         s.geometry = collision_detection::createCollisionGeometry(
-            link_model->getShapes()[shape_index], link_model, shape_index);
+            link_model->getShapes()[shape_index], link_model,
+            static_cast<int>(shape_index));
       }
       // LOG("b");
     }
@@ -175,8 +182,7 @@ void TouchGoal::describe(GoalContext& context) const {
 
 double TouchGoal::evaluate(const GoalContext& context) const {
   double dmin = DBL_MAX;
-  context.getTempVector().resize(1);
-  auto& last_collision_vertex = context.getTempVector()[0];
+  size_t last_collision_vertex = 0;
   auto& fb = context.getLinkFrame();
   size_t link_index = link_model->getLinkIndex();
   auto& collision_link = collision_model->collision_links[link_index];

src/ik_base.hpp

@@ -127,7 +127,7 @@ struct IKBase : Random {
   RobotFK model_;
   RobotInfo modelInfo_;
   IKParams params_;
-  int thread_index_;
+  size_t thread_index_;
   Problem problem_;
   std::vector<Frame> null_tip_frames_;
   volatile int canceled_;
@@ -139,7 +139,7 @@ struct IKBase : Random {
   virtual void setParams([[maybe_unused]] const IKParams&) {}
 
   IKBase(const IKParams& params)
-      : Random(params.random_seed),
+      : Random(static_cast<long unsigned int>(params.random_seed)),
         model_(params.robot_model),
         modelInfo_(params.robot_model),
         params_(params) {