Node logging for the rest of MoveIt (#2599)

moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp

@@ -47,13 +47,17 @@
 
 #include <moveit/collision_detection_bullet/collision_env_bullet.h>
 #include <moveit/collision_detection_bullet/bullet_integration/basic_types.h>
+#include <moveit/utils/logger.hpp>
 
 #include <urdf_parser/urdf_parser.h>
 #include <geometric_shapes/shape_operations.h>
 
 namespace cb = collision_detection_bullet;
 
-static const rclcpp::Logger TEST_LOGGER = rclcpp::get_logger("collision_detection.bullet_test");
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("collision_detection.bullet_test");
+}
 
 /** \brief Brings the panda robot in user defined home position */
 inline void setToHome(moveit::core::RobotState& panda_state)
@@ -254,7 +258,7 @@ TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
   ASSERT_FALSE(res.collision);
   res.clear();
 
-  RCLCPP_INFO(TEST_LOGGER, "Continuous to continuous collisions are not supported yet, therefore fail here.");
+  RCLCPP_INFO(getLogger(), "Continuous to continuous collisions are not supported yet, therefore fail here.");
   ASSERT_TRUE(res.collision);
   res.clear();
 }

moveit_core/collision_detection_fcl/test/test_fcl_env.cpp

@@ -41,6 +41,7 @@
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/utils/robot_model_test_utils.h>
+#include <moveit/utils/logger.hpp>
 
 #include <moveit/collision_detection_fcl/collision_common.h>
 #include <moveit/collision_detection_fcl/collision_env_fcl.h>
@@ -63,7 +64,10 @@ inline void setToHome(moveit::core::RobotState& panda_state)
   panda_state.update();
 }
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection_fcl.test.test_fcl_env");
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("moveit_collision_detection_fcl.test.test_fcl_env");
+}
 
 class CollisionDetectionEnvTest : public testing::Test
 {
@@ -262,7 +266,7 @@ TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf)
   res.clear();
 
   // c_env_->checkSelfCollision(req, res, state1, state2, *acm_);
-  RCLCPP_INFO(LOGGER, "Continuous to continuous collisions are not supported yet, therefore fail here.");
+  RCLCPP_INFO(getLogger(), "Continuous to continuous collisions are not supported yet, therefore fail here.");
   ASSERT_TRUE(res.collision);
   res.clear();
 }

moveit_core/collision_distance_field/src/collision_distance_field_types.cpp

@@ -42,13 +42,19 @@
 #include <rclcpp/logger.hpp>
 #include <rclcpp/time.hpp>
 #include <memory>
+#include <moveit/utils/logger.hpp>
 
 static const double EPSILON{ 0.0001 };
 
 namespace collision_detection
 {
-static const rclcpp::Logger LOGGER =
-    rclcpp::get_logger("moveit_collision_distance_field.collision_distance_field_types");
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("collision_distance_field_types");
+}
+}  // namespace
 
 std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body, Eigen::Isometry3d& relative_transform)
 {
@@ -158,7 +164,7 @@ bool getCollisionSphereGradients(const distance_field::DistanceField* distance_f
     double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
     if (!in_bounds && grad.norm() > EPSILON)
     {
-      RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
+      RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
       return true;
     }
 
@@ -216,7 +222,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f
 
     if (!in_bounds && grad.norm() > 0)
     {
-      RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds");
+      RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds");
       return true;
     }
 
@@ -243,7 +249,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f
     double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
     if (!in_bounds && (grad.norm() > 0))
     {
-      RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds");
+      RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds");
       return true;
     }
     if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
@@ -324,8 +330,8 @@ void BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes, c
   }
   bodies::mergeBoundingSpheres(bounding_spheres, relative_bounding_sphere_);
 
-  RCLCPP_DEBUG(LOGGER, "BodyDecomposition generated %zu collision spheres out of %zu shapes", collision_spheres_.size(),
-               shapes.size());
+  RCLCPP_DEBUG(getLogger(), "BodyDecomposition generated %zu collision spheres out of %zu shapes",
+               collision_spheres_.size(), shapes.size());
 }
 
 BodyDecomposition::~BodyDecomposition()
@@ -450,7 +456,7 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string&
   rclcpp::Clock ros_clock;
   if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
   {
-    RCLCPP_WARN(LOGGER, "Size mismatch between gradients %u and decompositions %u",
+    RCLCPP_WARN(getLogger(), "Size mismatch between gradients %u and decompositions %u",
                 static_cast<unsigned int>(gradients.size()),
                 static_cast<unsigned int>((posed_decompositions.size() + posed_vector_decompositions.size())));
     return;
@@ -484,12 +490,12 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string&
         }
         else
         {
-          RCLCPP_DEBUG(LOGGER, "Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm());
+          RCLCPP_DEBUG(getLogger(), "Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm());
         }
       }
       else
       {
-        RCLCPP_DEBUG(LOGGER, "Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id);
+        RCLCPP_DEBUG(getLogger(), "Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id);
       }
       arrow_mark.points.resize(2);
       if (i < posed_decompositions.size())
@@ -551,7 +557,7 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con
   rclcpp::Clock ros_clock;
   if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
   {
-    RCLCPP_WARN(LOGGER, "Size mismatch between gradients %zu and decompositions %zu", gradients.size(),
+    RCLCPP_WARN(getLogger(), "Size mismatch between gradients %zu and decompositions %zu", gradients.size(),
                 posed_decompositions.size() + posed_vector_decompositions.size());
     return;
   }

moveit_core/constraint_samplers/test/pr2_arm_ik.cpp

@@ -35,6 +35,7 @@
 /* Author: Sachin Chitta, E. Gil Jones */
 
 #include <angles/angles.h>
+#include <moveit/utils/logger.hpp>
 #include "pr2_arm_ik.h"
 
 /**** List of angles (for reference) *******
@@ -49,7 +50,13 @@
 using namespace angles;
 namespace pr2_arm_kinematics
 {
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constaint_samplers.test.pr2_arm_ik");
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("moveit_constaint_samplers.test.pr2_arm_ik");
+}
+}  // namespace
 
 PR2ArmIK::PR2ArmIK()
 {
@@ -69,11 +76,11 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string&
     {
       if (link->parent_joint)
       {
-        RCLCPP_ERROR(LOGGER, "Could not find joint: %s", link->parent_joint->name.c_str());
+        RCLCPP_ERROR(getLogger(), "Could not find joint: %s", link->parent_joint->name.c_str());
       }
       else
       {
-        RCLCPP_ERROR(LOGGER, "Link %s has no parent joint", link->name.c_str());
+        RCLCPP_ERROR(getLogger(), "Link %s has no parent joint", link->name.c_str());
       }
       return false;
     }
@@ -82,7 +89,8 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string&
       link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform);
       angle_multipliers_.push_back(joint->axis.x * fabs(joint->axis.x) + joint->axis.y * fabs(joint->axis.y) +
                                    joint->axis.z * fabs(joint->axis.z));
-      RCLCPP_DEBUG(LOGGER, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, joint->axis.z);
+      RCLCPP_DEBUG(getLogger(), "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y,
+                   joint->axis.z);
       if (joint->type != urdf::Joint::CONTINUOUS)
       {
         if (joint->safety)
@@ -101,7 +109,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string&
           {
             min_angles_.push_back(0.0);
             max_angles_.push_back(0.0);
-            RCLCPP_WARN(LOGGER, "No joint limits or joint '%s'", joint->name.c_str());
+            RCLCPP_WARN(getLogger(), "No joint limits or joint '%s'", joint->name.c_str());
           }
         }
         continuous_joint_.push_back(false);
@@ -133,7 +141,8 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string&
 
   if (num_joints != 7)
   {
-    RCLCPP_ERROR(LOGGER, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), tip_name.c_str());
+    RCLCPP_ERROR(getLogger(), "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(),
+                 tip_name.c_str());
     return false;
   }
 

moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp

@@ -41,14 +41,21 @@
 #include <cmath>
 
 #include <moveit/robot_model/robot_model.h>
+#include <moveit/utils/logger.hpp>
 #include "pr2_arm_kinematics_plugin.h"
 
 using namespace KDL;
 using namespace std;
 
 namespace pr2_arm_kinematics
 {
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin");
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin");
+}
+}  // namespace
 
 bool PR2ArmIKSolver::getCount(int& count, int max_count, int min_count)
 {
@@ -110,7 +117,7 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i
   if (free_angle_ == 0)
   {
     if (verbose)
-      RCLCPP_WARN(LOGGER, "Solving with %f", q_init(0));
+      RCLCPP_WARN(getLogger(), "Solving with %f", q_init(0));
     pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
   }
   else
@@ -128,14 +135,12 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i
   {
     if (verbose)
     {
-      RCLCPP_WARN(LOGGER, "Solution : %d", static_cast<int>(solution_ik.size()));
+      RCLCPP_WARN(getLogger(), "Solution : %d", static_cast<int>(solution_ik.size()));
 
       for (int j = 0; j < static_cast<int>(solution_ik[i].size()); ++j)
       {
-        RCLCPP_WARN(LOGGER, "%d: %f", j, solution_ik[i][j]);
+        RCLCPP_WARN(getLogger(), "%d: %f", j, solution_ik[i][j]);
       }
-      RCLCPP_WARN(LOGGER, " ");
-      RCLCPP_WARN(LOGGER, " ");
     }
     double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
     if (tmp_distance < min_distance)
@@ -175,7 +180,8 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame&
       (initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_);
   if (verbose)
   {
-    RCLCPP_WARN(LOGGER, "%f %f %f %d %d \n\n", initial_guess, pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,
+    RCLCPP_WARN(getLogger(), "%f %f %f %d %d \n\n", initial_guess,
+                pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,
                 pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments,
                 num_negative_increments);
   }
@@ -187,17 +193,17 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame&
       return -1;
     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
     if (verbose)
-      RCLCPP_WARN(LOGGER, "%d, %f", count, q_init(free_angle_));
+      RCLCPP_WARN(getLogger(), "%d, %f", count, q_init(free_angle_));
     loop_time = rclcpp::Clock(RCL_ROS_TIME).now().seconds() - start_time.seconds();
   }
   if (loop_time >= timeout)
   {
-    RCLCPP_WARN(LOGGER, "IK Timed out in %f seconds", timeout);
+    RCLCPP_WARN(getLogger(), "IK Timed out in %f seconds", timeout);
     return TIMED_OUT;
   }
   else
   {
-    RCLCPP_WARN(LOGGER, "No IK solution was found");
+    RCLCPP_WARN(getLogger(), "No IK solution was found");
     return NO_IK_SOLUTION;
   }
   return NO_IK_SOLUTION;
@@ -210,12 +216,13 @@ bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name
   KDL::Tree tree;
   if (!kdl_parser::treeFromUrdfModel(model, tree))
   {
-    RCLCPP_ERROR(LOGGER, "Could not initialize tree object");
+    RCLCPP_ERROR(getLogger(), "Could not initialize tree object");
     return false;
   }
   if (!tree.getChain(root_name, tip_name, kdl_chain))
   {
-    RCLCPP_ERROR(LOGGER, "Could not initialize chain object for base %s tip %s", root_name.c_str(), tip_name.c_str());
+    RCLCPP_ERROR(getLogger(), "Could not initialize chain object for base %s tip %s", root_name.c_str(),
+                 tip_name.c_str());
     return false;
   }
   return true;
@@ -275,11 +282,11 @@ bool PR2ArmKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node,
   std::string xml_string;
   dimension_ = 7;
 
-  RCLCPP_WARN(LOGGER, "Loading KDL Tree");
+  RCLCPP_WARN(getLogger(), "Loading KDL Tree");
   if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_))
   {
     active_ = false;
-    RCLCPP_ERROR(LOGGER, "Could not load kdl tree");
+    RCLCPP_ERROR(getLogger(), "Could not load kdl tree");
   }
   jnt_to_pose_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(kdl_chain_);
   free_angle_ = 2;
@@ -288,7 +295,7 @@ bool PR2ArmKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node,
       *robot_model.getURDF(), base_frame_, tip_frames_[0], search_discretization, free_angle_);
   if (!pr2_arm_ik_solver_->active_)
   {
-    RCLCPP_ERROR(LOGGER, "Could not load ik");
+    RCLCPP_ERROR(getLogger(), "Could not load ik");
     active_ = false;
   }
   else
@@ -301,17 +308,17 @@ bool PR2ArmKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node,
     {
       for (const std::string& joint_name : ik_solver_info_.joint_names)
       {
-        RCLCPP_WARN(LOGGER, "PR2Kinematics:: joint name: %s", joint_name.c_str());
+        RCLCPP_WARN(getLogger(), "PR2Kinematics:: joint name: %s", joint_name.c_str());
       }
       for (const std::string& link_name : ik_solver_info_.link_names)
       {
-        RCLCPP_WARN(LOGGER, "PR2Kinematics can solve IK for %s", link_name.c_str());
+        RCLCPP_WARN(getLogger(), "PR2Kinematics can solve IK for %s", link_name.c_str());
       }
       for (const std::string& link_name : fk_solver_info_.link_names)
       {
-        RCLCPP_WARN(LOGGER, "PR2Kinematics can solve FK for %s", link_name.c_str());
+        RCLCPP_WARN(getLogger(), "PR2Kinematics can solve FK for %s", link_name.c_str());
       }
-      RCLCPP_WARN(LOGGER, "PR2KinematicsPlugin::active for %s", group_name.c_str());
+      RCLCPP_WARN(getLogger(), "PR2KinematicsPlugin::active for %s", group_name.c_str());
     }
     active_ = true;
   }
@@ -335,7 +342,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik
 {
   if (!active_)
   {
-    RCLCPP_ERROR(LOGGER, "kinematics not active");
+    RCLCPP_ERROR(getLogger(), "kinematics not active");
     error_code.val = error_code.PLANNING_FAILED;
     return false;
   }
@@ -375,7 +382,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik
   }
   else
   {
-    RCLCPP_WARN(LOGGER, "An IK solution could not be found");
+    RCLCPP_WARN(getLogger(), "An IK solution could not be found");
     error_code.val = error_code.NO_IK_SOLUTION;
     return false;
   }
@@ -423,7 +430,7 @@ const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
 {
   if (!active_)
   {
-    RCLCPP_ERROR(LOGGER, "kinematics not active");
+    RCLCPP_ERROR(getLogger(), "kinematics not active");
   }
   return ik_solver_info_.joint_names;
 }
@@ -432,7 +439,7 @@ const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
 {
   if (!active_)
   {
-    RCLCPP_ERROR(LOGGER, "kinematics not active");
+    RCLCPP_ERROR(getLogger(), "kinematics not active");
   }
   return fk_solver_info_.link_names;
 }