Code refactoring

zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp

@@ -55,7 +55,7 @@ void RgbdSensorsDemuxNodelet::onInit()
   NODELET_INFO_STREAM(" * Subscribed to topic: " << mSub.getTopic().c_str());
 }
 
-void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr &msg)
+void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr& msg)
 {
   if (!msg->rgb.header.stamp.isZero())
   {

zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp

@@ -209,10 +209,10 @@ void RgbdSensorsSyncNodelet::readParameters()
   NODELET_INFO(" * sub_mag -> %s", mUseMag ? "true" : "false");
 }
 
-void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb,
-                                          const sensor_msgs::ImageConstPtr &depth,
-                                          const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                          const sensor_msgs::CameraInfoConstPtr &depthCameraInfo)
+void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr& rgb,
+                                          const sensor_msgs::ImageConstPtr& depth,
+                                          const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                          const sensor_msgs::CameraInfoConstPtr& depthCameraInfo)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -251,20 +251,22 @@ void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb,
 
   if (rgbStamp != rgb->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec());
   }
 }
 
-void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &rgb,
-                                             const sensor_msgs::ImageConstPtr &depth,
-                                             const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                             const sensor_msgs::CameraInfoConstPtr &depthCameraInfo,
-                                             const sensor_msgs::ImuConstPtr &imu)
+void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr& rgb,
+                                             const sensor_msgs::ImageConstPtr& depth,
+                                             const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                             const sensor_msgs::CameraInfoConstPtr& depthCameraInfo,
+                                             const sensor_msgs::ImuConstPtr& imu)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -309,20 +311,22 @@ void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &r
 
   if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f IMU=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f IMU=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec());
   }
 }
 
-void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &rgb,
-                                             const sensor_msgs::ImageConstPtr &depth,
-                                             const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                             const sensor_msgs::CameraInfoConstPtr &depthCameraInfo,
-                                             const sensor_msgs::MagneticFieldConstPtr &mag)
+void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr& rgb,
+                                             const sensor_msgs::ImageConstPtr& depth,
+                                             const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                             const sensor_msgs::CameraInfoConstPtr& depthCameraInfo,
+                                             const sensor_msgs::MagneticFieldConstPtr& mag)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -367,21 +371,23 @@ void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &r
 
   if (rgbStamp != rgb->header.stamp.toSec() || magStamp != mag->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f MAG=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec(), magStamp, mag->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f MAG=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec(), magStamp, mag->header.stamp.toSec());
   }
 }
 
-void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb,
-                                          const sensor_msgs::ImageConstPtr &depth,
-                                          const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                          const sensor_msgs::CameraInfoConstPtr &depthCameraInfo,
-                                          const sensor_msgs::ImuConstPtr &imu,
-                                          const sensor_msgs::MagneticFieldConstPtr &mag)
+void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr& rgb,
+                                          const sensor_msgs::ImageConstPtr& depth,
+                                          const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                          const sensor_msgs::CameraInfoConstPtr& depthCameraInfo,
+                                          const sensor_msgs::ImuConstPtr& imu,
+                                          const sensor_msgs::MagneticFieldConstPtr& mag)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -431,13 +437,14 @@ void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb,
   if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec() ||
       magStamp != mag->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f IMU=%f->%f MAG=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec(), magStamp,
-                  mag->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f IMU=%f->%f MAG=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec(), magStamp, mag->header.stamp.toSec());
   }
 }
 

zed_nodelets/src/tools/include/sl_tools.h

@@ -76,14 +76,13 @@ bool isZEDX(sl::MODEL camModel);
  *  \param poly the ROI polygon. Coordinates must be normalized from 0.0 to 1.0
  *  \param out_roi the `sl::Mat` containing the ROI
  */
-bool generateROI(const std::vector<sl::float2> & poly, sl::Mat & out_roi);
+bool generateROI(const std::vector<sl::float2>& poly, sl::Mat& out_roi);
 
 /*! \brief Parse a vector of vector of floats from a string.
  *  \param input
  *  \param error_return
  *  Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */
-std::vector<std::vector<float>> parseStringVector(
-  const std::string & input, std::string & error_return);
+std::vector<std::vector<float>> parseStringVector(const std::string& input, std::string& error_return);
 
 /*! \brief sl::Mat to ros message conversion
  * \param imgMsgPtr : the image topic message to publish

zed_nodelets/src/tools/src/sl_tools.cpp

@@ -39,12 +39,11 @@ bool file_exist(const std::string& name)
 namespace fs = std::experimental::filesystem;
 std::string resolveFilePath(std::string file_path)
 {
-  if(file_path.empty())
+  if (file_path.empty())
   {
     return file_path;
   }
 
-
   std::string abs_path = file_path;
   if (file_path[0] == '~')
   {
@@ -74,7 +73,7 @@ std::string resolveFilePath(std::string file_path)
       return std::string();
     }
   }
-  else if(file_path[0] != '/')
+  else if (file_path[0] != '/')
   {
     fs::path current_path = fs::current_path();
     abs_path = current_path.string() + "/" + file_path;
@@ -123,37 +122,43 @@ ros::Time slTime2Ros(sl::Timestamp t)
 
 bool isZED(sl::MODEL camModel)
 {
-  if (camModel == sl::MODEL::ZED) {
+  if (camModel == sl::MODEL::ZED)
+  {
     return true;
   }
   return false;
 }
 
 bool isZEDM(sl::MODEL camModel)
 {
-  if (camModel == sl::MODEL::ZED_M) {
+  if (camModel == sl::MODEL::ZED_M)
+  {
     return true;
   }
   return false;
 }
 
 bool isZED2OrZED2i(sl::MODEL camModel)
 {
-  if (camModel == sl::MODEL::ZED2) {
+  if (camModel == sl::MODEL::ZED2)
+  {
     return true;
   }
-  if (camModel == sl::MODEL::ZED2i) {
+  if (camModel == sl::MODEL::ZED2i)
+  {
     return true;
   }
   return false;
 }
 
 bool isZEDX(sl::MODEL camModel)
 {
-  if (camModel == sl::MODEL::ZED_X) {
+  if (camModel == sl::MODEL::ZED_X)
+  {
     return true;
   }
-  if (camModel == sl::MODEL::ZED_XM) {
+  if (camModel == sl::MODEL::ZED_XM)
+  {
     return true;
   }
   return false;
@@ -351,26 +356,26 @@ std::vector<std::string> split_string(const std::string& s, char seperator)
   return output;
 }
 
-inline bool contains(std::vector<sl::float2> & poly, sl::float2 test)
+inline bool contains(std::vector<sl::float2>& poly, sl::float2 test)
 {
   int i, j;
   bool c = false;
   const int nvert = poly.size();
-  for (i = 0, j = nvert - 1; i < nvert; j = i++) {
-    if (
-      ((poly[i].y > test.y) != (poly[j].y > test.y)) &&
-      (test.x <
-      (poly[j].x - poly[i].x) * (test.y - poly[i].y) / (poly[j].y - poly[i].y) + poly[i].x))
+  for (i = 0, j = nvert - 1; i < nvert; j = i++)
+  {
+    if (((poly[i].y > test.y) != (poly[j].y > test.y)) &&
+        (test.x < (poly[j].x - poly[i].x) * (test.y - poly[i].y) / (poly[j].y - poly[i].y) + poly[i].x))
     {
       c = !c;
     }
   }
   return c;
 }
 
-bool generateROI(const std::vector<sl::float2> & poly, sl::Mat & out_roi)
+bool generateROI(const std::vector<sl::float2>& poly, sl::Mat& out_roi)
 {
-  if (poly.size() < 3) {
+  if (poly.size() < 3)
+  {
     out_roi = sl::Mat();
     return false;
   }
@@ -387,15 +392,18 @@ bool generateROI(const std::vector<sl::float2> & poly, sl::Mat & out_roi)
   // std::cerr << "Image resolution: " << w << "x" << h << std::endl;
   std::vector<sl::float2> poly_img;
   size_t idx = 0;
-  for (auto & it : poly) {
+  for (auto& it : poly)
+  {
     sl::float2 pt;
     pt.x = it.x * w;
     pt.y = it.y * h;
 
-    if (pt.x >= w) {
+    if (pt.x >= w)
+    {
       pt.x = (w - 1);
     }
-    if (pt.y >= h) {
+    if (pt.y >= h)
+    {
       pt.y = (h - 1);
     }
 
@@ -406,11 +414,14 @@ bool generateROI(const std::vector<sl::float2> & poly, sl::Mat & out_roi)
   // <---- De-normalize coordinates
 
   // ----> Unset ROI pixels outside the polygon
-  //std::cerr << "Unset ROI pixels outside the polygon" << std::endl;
-  //std::cerr << "Set mask" << std::endl;
-  for (int v = 0; v < h; v++) {
-    for (int u = 0; u < w; u++) {
-      if (!contains(poly_img, sl::float2(u, v))) {
+  // std::cerr << "Unset ROI pixels outside the polygon" << std::endl;
+  // std::cerr << "Set mask" << std::endl;
+  for (int v = 0; v < h; v++)
+  {
+    for (int u = 0; u < w; u++)
+    {
+      if (!contains(poly_img, sl::float2(u, v)))
+      {
         out_roi.setValue<sl::uchar1>(u, v, 0, sl::MEM::CPU);
       }
     }
@@ -422,21 +433,23 @@ bool generateROI(const std::vector<sl::float2> & poly, sl::Mat & out_roi)
   return true;
 }
 
-std::vector<std::vector<float>> parseStringVector(
-  const std::string & input, std::string & error_return)
+std::vector<std::vector<float>> parseStringVector(const std::string& input, std::string& error_return)
 {
   std::vector<std::vector<float>> result;
 
   std::stringstream input_ss(input);
   int depth = 0;
   std::vector<float> current_vector;
-  while (!!input_ss && !input_ss.eof()) {
-    switch (input_ss.peek()) {
+  while (!!input_ss && !input_ss.eof())
+  {
+    switch (input_ss.peek())
+    {
       case EOF:
         break;
       case '[':
         depth++;
-        if (depth > 2) {
+        if (depth > 2)
+        {
           error_return = "Array depth greater than 2";
           return result;
         }
@@ -445,12 +458,14 @@ std::vector<std::vector<float>> parseStringVector(
         break;
       case ']':
         depth--;
-        if (depth < 0) {
+        if (depth < 0)
+        {
           error_return = "More close ] than open [";
           return result;
         }
         input_ss.get();
-        if (depth == 1) {
+        if (depth == 1)
+        {
           result.push_back(current_vector);
         }
         break;
@@ -460,24 +475,29 @@ std::vector<std::vector<float>> parseStringVector(
         input_ss.get();
         break;
       default:  // All other characters should be part of the numbers.
-        if (depth != 2) {
+        if (depth != 2)
+        {
           std::stringstream err_ss;
           err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
           error_return = err_ss.str();
           return result;
         }
         float value;
         input_ss >> value;
-        if (!!input_ss) {
+        if (!!input_ss)
+        {
           current_vector.push_back(value);
         }
         break;
     }
   }
 
-  if (depth != 0) {
+  if (depth != 0)
+  {
     error_return = "Unterminated vector string.";
-  } else {
+  }
+  else
+  {
     error_return = "";
   }