Converts float to double (#2343)

* Limiting the scope of variables #874 Limited the scope of variables in moveit_core/collision_detection * Update moveit_core/collision_detection/src/collision_octomap_filter.cpp Co-authored-by: AndyZe <[email protected]> * Update moveit_core/collision_detection/src/collision_octomap_filter.cpp Co-authored-by: AndyZe <[email protected]> * Update moveit_core/collision_detection/src/collision_octomap_filter.cpp Co-authored-by: AndyZe <[email protected]> * convert float to double * change double to float * Feedback fixes * Introduced variables removed from previous merge commit * Updated GL_Renderer function definitions with double instead of float * Changed update() function arguments to float since it is a derived virtual function and needs to be overriden * Fixed all override errors in visualization * *Fixed override errors in perception *Changed reinterpret_cast to double* from float* * change variable types to fit function definition * Fixed clang-tidy warnings * Fixed scope of reusable variables --------- Co-authored-by: Salah Soliman <[email protected]> Co-authored-by: AndyZe <[email protected]> Co-authored-by: Henning Kayser <[email protected]>
moveit · Sep 14, 2023 · 6482350 · 6482350
1 parent 27e5d0e
commit 6482350
Show file tree

Hide file tree

Showing 33 changed files with 109 additions and 110 deletions.
diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp
@@ -98,13 +98,13 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
       continue;
     }
 
-    double cell_size = 0;
     if (!object->shapes_.empty())
     {
       const shapes::ShapeConstPtr& shape = object->shapes_[0];
       const std::shared_ptr<const shapes::OcTree> shape_octree = std::dynamic_pointer_cast<const shapes::OcTree>(shape);
       if (shape_octree)
       {
+        double cell_size = 0;
         std::shared_ptr<const octomap::OcTree> octree = shape_octree->octree;
         cell_size = octree->getResolution();
         for (auto& contact_info : contact_vector)
@@ -174,7 +174,6 @@ bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const doub
                                   const double& r_multiple, const octomath::Vector3& contact_point,
                                   octomath::Vector3& normal, double& depth, const bool estimate_depth)
 {
-  double intensity;
   if (estimate_depth)
   {
     octomath::Vector3 surface_point;
@@ -191,6 +190,7 @@ bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const doub
   else  // just get normals, no depth
   {
     octomath::Vector3 gradient;
+    double intensity = 0;
     if (sampleCloud(cloud, spacing, r_multiple, contact_point, intensity, gradient))
     {
       normal = gradient.normalized();
@@ -211,11 +211,10 @@ bool findSurface(const octomap::point3d_list& cloud, const double& spacing, cons
                  const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point,
                  octomath::Vector3& normal)
 {
-  const double epsilon = 1e-10;
+  octomath::Vector3 p = seed, dp, gs;
   const int iterations = 10;
+  const double epsilon = 1e-10;
   double intensity = 0;
-
-  octomath::Vector3 p = seed, dp, gs;
   for (int i = 0; i < iterations; ++i)
   {
     if (!sampleCloud(cloud, spacing, r_multiple, p, intensity, gs))

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1058,7 +1058,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet
           RCLCPP_ERROR(LOGGER, "Failed to read pose from scene file");
           return false;
         }
-        float r, g, b, a;
+        double r, g, b, a;
         if (!(in >> r >> g >> b >> a))
         {
           RCLCPP_ERROR(LOGGER, "Improperly formatted color in scene geometry file");

diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp
@@ -173,7 +173,7 @@ bool FloatingJointModel::satisfiesPositionBounds(const double* values, const Bou
   if (values[2] < bounds[2].min_position_ - margin || values[2] > bounds[2].max_position_ + margin)
     return false;
   double norm_sqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
-  return fabs(norm_sqr - 1.0) <= std::numeric_limits<float>::epsilon() * 10.0;
+  return fabs(norm_sqr - 1.0) <= std::numeric_limits<double>::epsilon() * 10.0;
 }
 
 bool FloatingJointModel::normalizeRotation(double* values) const

diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
@@ -1287,7 +1287,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons
             if (shapes::constructMarkerFromShape(it.second->getShapes()[j].get(), att_mark))
             {
               // if the object is invisible (0 volume) we skip it
-              if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
+              if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<double>::epsilon())
                 continue;
               att_mark.pose = tf2::toMsg(it.second->getGlobalCollisionBodyTransforms()[j]);
               arr.markers.push_back(att_mark);
@@ -1313,7 +1313,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons
         if (!shapes::constructMarkerFromShape(link_model->getShapes()[j].get(), mark))
           continue;
         // if the object is invisible (0 volume) we skip it
-        if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
+        if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<double>::epsilon())
           continue;
         mark.pose =
             tf2::toMsg(global_collision_body_transforms_[link_model->getFirstCollisionBodyTransformIndex() + j]);
@@ -1488,7 +1488,7 @@ void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::Vect
   const Eigen::VectorXd& s = svd_of_j.singularValues();
 
   Eigen::VectorXd sinv = s;
-  static const double PINVTOLER = std::numeric_limits<float>::epsilon();
+  static const double PINVTOLER = std::numeric_limits<double>::epsilon();
   double maxsv = 0.0;
   for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
   {

diff --git a/...t_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/...t_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp
@@ -126,7 +126,7 @@ void ompl_interface::PoseModelStateSpace::copyState(ompl::base::State* destinati
 
 void ompl_interface::PoseModelStateSpace::sanityChecks() const
 {
-  ModelBasedStateSpace::sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(),
+  ModelBasedStateSpace::sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<double>::epsilon(),
                                      ~ompl::base::StateSpace::STATESPACE_TRIANGLE_INEQUALITY);
 }
 

diff --git a/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
@@ -51,7 +51,7 @@ const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component");
 const auto JOIN_THREAD_TIMEOUT = std::chrono::seconds(1);
 
 // If the trajectory progress reaches more than 0.X the global goal state is considered as reached
-constexpr float PROGRESS_THRESHOLD = 0.995;
+constexpr double PROGRESS_THRESHOLD = 0.995;
 }  // namespace
 
 LocalPlannerComponent::LocalPlannerComponent(const rclcpp::NodeOptions& options)

diff --git a/..._octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/..._octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h
@@ -103,7 +103,7 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
   std::unique_ptr<mesh_filter::MeshFilter<mesh_filter::StereoCameraModel> > mesh_filter_;
   std::unique_ptr<LazyFreeSpaceUpdater> free_space_updater_;
 
-  std::vector<float> x_cache_, y_cache_;
+  std::vector<double> x_cache_, y_cache_;
   double inv_fx_, inv_fy_, K0_, K2_, K4_, K5_;
   std::vector<unsigned int> filtered_labels_;
   rclcpp::Time last_depth_callback_start_;

diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
@@ -445,7 +445,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     debug_msg.step = w * sizeof(float);
     debug_msg.data.resize(img_size * sizeof(float));
-    mesh_filter_->getModelDepth(reinterpret_cast<float*>(&debug_msg.data[0]));
+    mesh_filter_->getModelDepth(reinterpret_cast<double*>(&debug_msg.data[0]));
     pub_model_depth_image_.publish(debug_msg, *info_msg);
 
     sensor_msgs::msg::Image filtered_depth_msg;
@@ -456,7 +456,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     filtered_depth_msg.step = w * sizeof(float);
     filtered_depth_msg.data.resize(img_size * sizeof(float));
-    mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_depth_msg.data[0]));
+    mesh_filter_->getFilteredDepth(reinterpret_cast<double*>(&filtered_depth_msg.data[0]));
     pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg);
 
     sensor_msgs::msg::Image label_msg;
@@ -488,7 +488,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     if (filtered_data.size() < img_size)
       filtered_data.resize(img_size);
 
-    mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_data[0]));
+    mesh_filter_->getFilteredDepth(reinterpret_cast<double*>(&filtered_data[0]));
     unsigned short* msg_data = reinterpret_cast<unsigned short*>(&filtered_msg.data[0]);
     for (std::size_t i = 0; i < img_size; ++i)
     {

diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h
@@ -70,7 +70,7 @@ class GLRenderer
    * \param[in] near distance of the near clipping plane in meters
    * \param[in] far distance of the far clipping plane in meters
    */
-  GLRenderer(unsigned width, unsigned height, float near = 0.1, float far = 10.0);
+  GLRenderer(unsigned width, unsigned height, double near = 0.1, double far = 10.0);
 
   /** \brief destructor, destroys frame buffer objects and OpenGL context*/
   ~GLRenderer();
@@ -106,7 +106,7 @@ class GLRenderer
    * \author Suat Gedikli ([email protected])
    * \param[out] buffer pointer to memory where the depth values need to be stored
    */
-  void getDepthBuffer(float* buffer) const;
+  void getDepthBuffer(double* buffer) const;
 
   /**
    * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context.
@@ -135,29 +135,29 @@ class GLRenderer
    * \param[in] cx x component of principal point
    * \param[in] cy y component of principal point
    */
-  void setCameraParameters(float fx, float fy, float cx, float cy);
+  void setCameraParameters(double fx, double fy, double cx, double cy);
 
   /**
    * \brief sets the near and far clipping plane distances in meters
    * \author Suat Gedikli ([email protected])
    * \param[in] near distance of the near clipping plane in meters
    * \param[in] far distance of the far clipping plane in meters
    */
-  void setClippingRange(float near, float far);
+  void setClippingRange(double near, double far);
 
   /**
    * \brief returns the distance of the near clipping plane in meters
    * \author Suat Gedikli ([email protected])
    * \return distance of near clipping plane in meters
    */
-  const float& getNearClippingDistance() const;
+  const double& getNearClippingDistance() const;
 
   /**
    * \brief returns the distance of the far clipping plane in meters
    * \author Suat Gedikli ([email protected])
    * \return distance of the far clipping plane in meters
    */
-  const float& getFarClippingDistance() const;
+  const double& getFarClippingDistance() const;
 
   /**
    * \brief returns the width of the frame buffer objectsin pixels
@@ -281,10 +281,10 @@ class GLRenderer
   GLuint program_;
 
   /** \brief distance of near clipping plane in meters*/
-  float near_;
+  double near_;
 
   /** \brief distance of far clipping plane in meters*/
-  float far_;
+  double far_;
 
   /** \brief focal length in x-direction of camera in pixels*/
   float fx_;
@@ -299,11 +299,11 @@ class GLRenderer
   float cy_;
 
   /** \brief map from thread id to OpenGL context */
-  static std::map<std::thread::id, std::pair<unsigned, GLuint> > context_;
+  static std::map<std::thread::id, std::pair<unsigned, GLuint> > context;
 
   /* \brief lock for context map */
-  static std::mutex context_lock_;
+  static std::mutex context_lock;
 
-  static bool glutInitialized_;
+  static bool glut_initialized;
 };
 }  // namespace mesh_filter
diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h
@@ -131,7 +131,7 @@ class MeshFilterBase
    * \author Suat Gedikli ([email protected])
    * \param[out] depth pointer to buffer to be filled with depth values.
    */
-  void getFilteredDepth(float* depth) const;
+  void getFilteredDepth(double* depth) const;
 
   /**
    * \brief retrieves the labels of the rendered model
@@ -149,7 +149,7 @@ class MeshFilterBase
    * \author Suat Gedikli ([email protected])
    * \param[out] depth pointer to buffer to be filled with depth values.
    */
-  void getModelDepth(float* depth) const;
+  void getModelDepth(double* depth) const;
 
   /**
    * \brief set the shadow threshold. points that are further away than the rendered model are filtered out.

diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h
@@ -103,13 +103,13 @@ class SensorModel
      * \brief transforms depth values from rendered model to metric depth values
      * \param[in,out] depth pointer to floating point depth buffer
      */
-    virtual void transformModelDepthToMetricDepth(float* depth) const;
+    virtual void transformModelDepthToMetricDepth(double* depth) const;
 
     /**
      * \brief transforms depth values from filtered depth to metric depth values
      * \param[in,out] depth pointer to floating point depth buffer
      */
-    virtual void transformFilteredDepthToMetricDepth(float* depth) const;
+    virtual void transformFilteredDepthToMetricDepth(double* depth) const;
 
     /**
      * \brief sets  the image size

diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp
@@ -113,7 +113,7 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
   params.setCameraParameters(info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]);
   params.setImageSize(depth_msg->width, depth_msg->height);
 
-  const float* src = (const float*)&depth_msg->data[0];
+  const double* src = (const double*)&depth_msg->data[0];
   //*
   static unsigned data_size = 0;
   static unsigned short* data = nullptr;
@@ -135,7 +135,7 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
     if (static_cast<uint32_t>(filtered_depth_ptr_->image.cols) != depth_msg->width ||
         static_cast<uint32_t>(filtered_depth_ptr_->image.rows) != depth_msg->height)
       filtered_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
-    mesh_filter_->getFilteredDepth((float*)filtered_depth_ptr_->image.data);
+    mesh_filter_->getFilteredDepth((double*)filtered_depth_ptr_->image.data);
     pub_filtered_depth_image_.publish(filtered_depth_ptr_->toImageMsg(), info_msg);
   }
 
@@ -148,7 +148,7 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
     if (static_cast<uint32_t>(model_depth_ptr_->image.cols) != depth_msg->width ||
         static_cast<uint32_t>(model_depth_ptr_->image.rows) != depth_msg->height)
       model_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
-    mesh_filter_->getModelDepth((float*)model_depth_ptr_->image.data);
+    mesh_filter_->getModelDepth((double*)model_depth_ptr_->image.data);
     pub_model_depth_image_.publish(model_depth_ptr_->toImageMsg(), info_msg);
   }