distance_field, adapt logging moveit#21

AcutronicRobotics · Mar 31, 2019 · c1eb061 · c1eb061
1 parent 4d0dca1
commit c1eb061
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 17 deletions.
diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp
@@ -40,8 +40,9 @@
 #include <tf2_eigen/tf2_eigen.h>
 #include <octomap/octomap.h>
 #include <octomap/OcTree.h>
+#include "rclcpp/rclcpp.hpp"
 
-rclcpp::Logger logger = rclcpp::get_logger("distance_field");
+rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("distance_field");
 
 namespace distance_field
 {
@@ -202,7 +203,7 @@ bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isom
     const shapes::OcTree* oc = dynamic_cast<const shapes::OcTree*>(shape);
     if (!oc)
     {
-      RCLCPP_ERROR(logger, "Problem dynamic casting shape that claims to be OcTree");
+      RCLCPP_ERROR(LOGGER, "Problem dynamic casting shape that claims to be OcTree");
       return false;
     }
     getOcTreePoints(oc->octree.get(), points);
@@ -290,7 +291,7 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is
 {
   if (shape->type == shapes::OCTREE)
   {
-    RCLCPP_WARN(logger, "Move shape not supported for Octree");
+    RCLCPP_WARN(LOGGER, "Move shape not supported for Octree");
     return;
   }
   bodies::Body* body = bodies::createBodyFromShape(shape);

diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp
@@ -39,8 +39,9 @@
 #include <boost/iostreams/filtering_stream.hpp>
 #include <boost/iostreams/copy.hpp>
 #include <boost/iostreams/filter/zlib.hpp>
+#include "rclcpp/rclcpp.hpp"
 
-rclcpp::Logger logger = rclcpp::get_logger("distance_field");
+rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("distance_field");
 
 namespace distance_field
 {
@@ -103,26 +104,26 @@ int PropagationDistanceField::eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i
 
 void PropagationDistanceField::print(const VoxelSet& set)
 {
-  RCLCPP_DEBUG(logger, "[");
+  RCLCPP_DEBUG(LOGGER, "[");
   VoxelSet::const_iterator it;
   for (it = set.begin(); it != set.end(); ++it)
   {
     Eigen::Vector3i loc1 = *it;
-    RCLCPP_DEBUG(logger, "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z());
+    RCLCPP_DEBUG(LOGGER, "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z());
   }
-  RCLCPP_DEBUG(logger, "] size=%u\n", (unsigned int)set.size());
+  RCLCPP_DEBUG(LOGGER, "] size=%u\n", (unsigned int)set.size());
 }
 
 void PropagationDistanceField::print(const EigenSTL::vector_Vector3d& points)
 {
-  RCLCPP_DEBUG(logger, "[");
+  RCLCPP_DEBUG(LOGGER, "[");
   EigenSTL::vector_Vector3d::const_iterator it;
   for (it = points.begin(); it != points.end(); ++it)
   {
     Eigen::Vector3d loc1 = *it;
-    RCLCPP_DEBUG(logger, "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z());
+    RCLCPP_DEBUG(LOGGER, "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z());
   }
-  RCLCPP_DEBUG(logger, "] size=%u\n", (unsigned int)points.size());
+  RCLCPP_DEBUG(LOGGER, "] size=%u\n", (unsigned int)points.size());
 }
 
 void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
@@ -168,19 +169,19 @@ void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector
     {
       new_not_in_current.push_back(new_not_old[i]);
     }
-    // RCLCPP_INFO(logger, "Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z());
+    // RCLCPP_INFO(LOGGER, "Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z());
   }
 
   removeObstacleVoxels(old_not_new);
   addNewObstacleVoxels(new_not_in_current);
 
-  // RCLCPP_DEBUG(logger,  "new=" );
+  // RCLCPP_DEBUG(LOGGER,  "new=" );
   // print(points_added);
-  // RCLCPP_DEBUG(logger,  "removed=" );
+  // RCLCPP_DEBUG(LOGGER,  "removed=" );
   // print(points_removed);
-  // RCLCPP_DEBUG(logger,  "obstacle_voxel_locations_=" );
+  // RCLCPP_DEBUG(LOGGER,  "obstacle_voxel_locations_=" );
   // print(object_voxel_locations_);
-  // RCLCPP_DEBUG(logger, "");
+  // RCLCPP_DEBUG(LOGGER, "");
 }
 
 void PropagationDistanceField::addPointsToField(const EigenSTL::vector_Vector3d& points)
@@ -417,7 +418,7 @@ void PropagationDistanceField::propagatePositive()
       // This will never happen.  update_direction_ is always set before voxel is added to bucket queue.
       if (vptr->update_direction_ < 0 || vptr->update_direction_ > 26)
       {
-        RCLCPP_ERROR(logger, "PROGRAMMING ERROR: Invalid update direction detected: %d",
+        RCLCPP_ERROR(LOGGER, "PROGRAMMING ERROR: Invalid update direction detected: %d",
                         vptr->update_direction_);
         continue;
       }
@@ -476,7 +477,7 @@ void PropagationDistanceField::propagateNegative()
       // negative_bucket_queue_.
       if (vptr->negative_update_direction_ < 0 || vptr->negative_update_direction_ > 26)
       {
-        RCLCPP_ERROR(logger, "PROGRAMMING ERROR: Invalid update direction detected: %d",
+        RCLCPP_ERROR(LOGGER, "PROGRAMMING ERROR: Invalid update direction detected: %d",
                         vptr->update_direction_);
         continue;
       }