From c1eb061d0c0772b17cf56f4c900907161fa88d31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 31 Mar 2019 18:31:37 +0200 Subject: [PATCH] distance_field, adapt logging https://github.com/ros-planning/moveit2/issues/21 --- .../distance_field/src/distance_field.cpp | 7 +++-- .../src/propagation_distance_field.cpp | 29 ++++++++++--------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 17f910e6c8..0c61592458 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -40,8 +40,9 @@ #include #include #include +#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(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 index 7b5a2093b5..37a7e6f13c 100644 --- 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 #include #include +#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; }