collision_detection, change logging

Follows from moveit#21

Changes in the code structure were required given the new
macros used for logging.

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -45,6 +45,7 @@
 #include <set>
 #include <Eigen/Core>
 #include <moveit/robot_model/robot_model.h>
+#include <moveit/collision_detection/collision_log.h>
 
 namespace collision_detection
 {

moveit_core/collision_detection/include/moveit/collision_detection/collision_log.h

@@ -0,0 +1,50 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Acutronic Robotics AG
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Víctor Mayoral Vilches */
+
+#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_LOG_
+#define MOVEIT_COLLISION_DETECTION_COLLISION_LOG_
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace collision_detection
+{
+// Logger
+rclcpp::Logger logger_collision_detection = rclcpp::get_logger("collision_detection");
+
+};
+
+
+#endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h

@@ -38,6 +38,7 @@
 #define MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_
 
 #include <moveit/collision_detection/collision_common.h>
+#include <moveit/collision_detection/collision_log.h>
 #include <moveit/macros/class_forward.h>
 #include <moveit_msgs/msg/allowed_collision_matrix.hpp>
 #include <boost/function.hpp>

moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h

@@ -38,6 +38,7 @@
 #define MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_
 
 #include <moveit/collision_detection/collision_matrix.h>
+#include <moveit/collision_detection/collision_log.h>
 #include <moveit/macros/class_forward.h>
 #include <moveit/robot_state/robot_state.h>
 #include <moveit_msgs/msg/link_padding.hpp>

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -38,7 +38,7 @@
 #define MOVEIT_COLLISION_DETECTION_WORLD_
 
 #include <moveit/macros/class_forward.h>
-
+#include <moveit/collision_detection/collision_log.h>
 #include <string>
 #include <vector>
 #include <map>

moveit_core/collision_detection/src/collision_matrix.cpp

@@ -37,11 +37,10 @@
 #include <moveit/collision_detection/collision_matrix.h>
 #include <boost/bind.hpp>
 #include <iomanip>
-#include <moveit/logging/logging.h>
-
 
 namespace collision_detection
 {
+
 AllowedCollisionMatrix::AllowedCollisionMatrix()
 {
 }
@@ -56,19 +55,22 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& n
 AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCollisionMatrix& msg)
 {
   if (msg.entry_names.size() != msg.entry_values.size() ||
-      msg.default_entry_names.size() != msg.default_entry_values.size())
-    ROS_ERROR_NAMED("collision_detection", "The number of links does not match the number of entries "
-                                           "in AllowedCollisionMatrix message");
+      msg.default_entry_names.size() != msg.default_entry_values.size()){
+        RCLCPP_ERROR(logger_collision_detection, "The number of links does not match the number of entries in AllowedCollisionMatrix message");
+      }
   else
   {
-    for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
-      if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
-        ROS_ERROR_NAMED("collision_detection",
-                        "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
-                        msg.entry_names[i].c_str());
-      else
-        for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j)
+    for (std::size_t i = 0; i < msg.entry_names.size(); ++i) {
+      if (msg.entry_values[i].enabled.size() != msg.entry_names.size()){
+        RCLCPP_ERROR(logger_collision_detection,
+          "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
+          msg.entry_names[i].c_str());
+      } else {
+        for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j){
           setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]);
+        }
+      }
+    }
 
     for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
       setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);

moveit_core/collision_detection/src/collision_octomap_filter.cpp

@@ -42,7 +42,6 @@
 #include <octomap/octomap.h>
 #include <geometric_shapes/shapes.h>
 #include <memory>
-#include <moveit/logging/logging.h>
 
 // static const double ISO_VALUE  = 0.5; // TODO magic number! (though, probably a good one).
 // static const double R_MULTIPLE = 1.5; // TODO magic number! (though, probably a good one).
@@ -65,12 +64,12 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
 {
   if (!object)
   {
-    ROS_ERROR_NAMED("collision_detection", "No valid Object passed in, cannot refine Normals!");
+    RCLCPP_ERROR(collision_detection::logger_collision_detection, "No valid Object passed in, cannot refine Normals!");
     return 0;
   }
   if (res.contact_count < 1)
   {
-    ROS_WARN_NAMED("collision_detection", "There do not appear to be any contacts, so there is nothing to refine!");
+    RCLCPP_WARN(collision_detection::logger_collision_detection, "There do not appear to be any contacts, so there is nothing to refine!");
     return 0;
   }
 
@@ -126,16 +125,16 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
             {
               count++;
               node_centers.push_back(pt);
-              // ROS_INFO_NAMED("collision_detection", "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
+              // RCLCPP_INFO(collision_detection::logger_collision_detection, "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
               //                          count, prob, pt.x(), pt.y(), pt.z());
             }
           }
-          // ROS_INFO_NAMED("collision_detection", "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells
+          // RCLCPP_INFO(collision_detection::logger_collision_detection, "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells
           // %d",
           //                          contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);
 
           // octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
-          // ROS_ERROR_NAMED("collision_detection", "bad stuff in collision_octomap_filter.cpp; need to port octomap
+          // RCLCPP_ERROR(collision_detection::logger_collision_detection, "bad stuff in collision_octomap_filter.cpp; need to port octomap
           // call for groovy");
 
           octomath::Vector3 n;
@@ -148,7 +147,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
             if (divergence > allowed_angle_divergence)
             {
               modified++;
-              // ROS_INFO_NAMED("collision_detection", "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f,
+              // RCLCPP_INFO(collision_detection::logger_collision_detection, "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f,
               // %.3f, %.3f]",
               //                          divergence, contact_normal.x(), contact_normal.y(), contact_normal.z(),
               //                          n.x(), n.y(), n.z());
@@ -270,7 +269,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
     }
     else
     {
-      ROS_ERROR_NAMED("collision_detection", "This should not be called!");
+      RCLCPP_ERROR(collision_detection::logger_collision_detection, "This should not be called!");
     }
 
     double f_val = 0;
@@ -296,7 +295,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
     }
     else
     {
-      ROS_ERROR_NAMED("collision_detection", "This should not be called!");
+      RCLCPP_ERROR(collision_detection::logger_collision_detection, "This should not be called!");
       double r_scaled = r / r;
       // TODO still need to address the scaling...
       f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1);

moveit_core/collision_detection/src/collision_robot.cpp

@@ -36,18 +36,17 @@
 
 #include <moveit/collision_detection/collision_robot.h>
 #include <limits>
-#include <moveit/logging/logging.h>
 
 static inline bool validateScale(double scale)
 {
   if (scale < std::numeric_limits<double>::epsilon())
   {
-    ROS_ERROR_NAMED("collision_detection", "Scale must be positive");
+    RCLCPP_ERROR(collision_detection::logger_collision_detection, "Scale must be positive");
     return false;
   }
   if (scale > std::numeric_limits<double>::max())
   {
-    ROS_ERROR_NAMED("collision_detection", "Scale must be finite");
+    RCLCPP_ERROR(collision_detection::logger_collision_detection, "Scale must be finite");
     return false;
   }
   return true;
@@ -57,12 +56,12 @@ static inline bool validatePadding(double padding)
 {
   if (padding < 0.0)
   {
-    ROS_ERROR_NAMED("collision_detection", "Padding cannot be negative");
+    RCLCPP_ERROR(collision_detection::logger_collision_detection, "Padding cannot be negative");
     return false;
   }
   if (padding > std::numeric_limits<double>::max())
   {
-    ROS_ERROR_NAMED("collision_detection", "Padding must be finite");
+    RCLCPP_ERROR(collision_detection::logger_collision_detection, "Padding must be finite");
     return false;
   }
   return true;

moveit_core/collision_detection/src/world.cpp

@@ -35,7 +35,6 @@
 /* Author: Acorn Pooley, Ioan Sucan */
 
 #include <moveit/collision_detection/world.h>
-#include <moveit/logging/logging.h>
 
 namespace collision_detection
 {
@@ -66,8 +65,7 @@ void World::addToObject(const std::string& id, const std::vector<shapes::ShapeCo
 {
   if (shapes.size() != poses.size())
   {
-    ROS_ERROR_NAMED("collision_detection", "Number of shapes and number of poses do not match. "
-                                           "Not adding this object to collision world.");
+    RCLCPP_ERROR(logger_collision_detection, "Number of shapes and number of poses do not match. Not adding this object to collision world.");
     return;
   }