From c42f73f6d4396259ab91e5b765403bbfce156f89 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 30 Oct 2023 20:05:22 -0600 Subject: [PATCH 1/8] Node logging in moveit_core Signed-off-by: Tyler Weaver --- doc/MIGRATION_GUIDE.md | 14 ++ .../collision_detection/CMakeLists.txt | 1 + .../src/allvalid/collision_env_allvalid.cpp | 89 ++++++----- .../src/collision_common.cpp | 17 ++- .../collision_detection/src/collision_env.cpp | 19 ++- .../src/collision_matrix.cpp | 16 +- .../src/collision_octomap_filter.cpp | 27 ++-- .../src/collision_plugin_cache.cpp | 16 +- moveit_core/collision_detection/src/world.cpp | 17 ++- .../collision_detection_bullet/CMakeLists.txt | 2 + .../bullet_integration/ros_bullet_utils.h | 2 + .../bullet_cast_bvh_manager.cpp | 9 +- .../bullet_discrete_bvh_manager.cpp | 9 +- .../src/bullet_integration/bullet_utils.cpp | 38 +++-- .../contact_checker_common.cpp | 7 +- .../bullet_integration/ros_bullet_utils.cpp | 11 +- .../src/collision_env_bullet.cpp | 13 +- .../collision_detection_fcl/CMakeLists.txt | 2 + .../src/collision_common.cpp | 59 +++++--- .../src/collision_env_fcl.cpp | 20 ++- .../collision_distance_field_types.h | 17 ++- .../collision_env_distance_field.h | 7 +- .../src/collision_common_distance_field.cpp | 16 +- .../src/collision_distance_field_types.cpp | 2 - .../src/collision_env_distance_field.cpp | 85 ++++++----- .../constraint_samplers/CMakeLists.txt | 1 + .../src/constraint_sampler.cpp | 12 +- .../src/constraint_sampler_manager.cpp | 45 +++--- .../src/constraint_sampler_tools.cpp | 14 +- .../src/default_constraint_samplers.cpp | 56 ++++--- .../src/union_constraint_sampler.cpp | 14 +- moveit_core/distance_field/CMakeLists.txt | 2 +- .../distance_field/src/distance_field.cpp | 15 +- .../src/propagation_distance_field.cpp | 27 ++-- moveit_core/dynamics_solver/CMakeLists.txt | 1 + .../dynamics_solver/src/dynamics_solver.cpp | 66 +++++---- moveit_core/exceptions/CMakeLists.txt | 1 + moveit_core/exceptions/src/exceptions.cpp | 6 +- .../src/kinematic_constraint.cpp | 94 ++++++------ .../kinematic_constraints/src/utils.cpp | 22 ++- moveit_core/kinematics_base/CMakeLists.txt | 2 +- .../moveit/kinematics_base/kinematics_base.h | 10 +- .../kinematics_base/src/kinematics_base.cpp | 18 ++- moveit_core/kinematics_metrics/CMakeLists.txt | 1 + .../src/kinematics_metrics.cpp | 18 ++- .../src/planning_interface.cpp | 16 +- .../planning_scene/src/planning_scene.cpp | 121 ++++++++------- moveit_core/robot_model/CMakeLists.txt | 1 + .../robot_model/src/floating_joint_model.cpp | 10 +- .../robot_model/src/joint_model_group.cpp | 38 +++-- moveit_core/robot_model/src/robot_model.cpp | 138 +++++++++--------- .../src/cartesian_interpolator.cpp | 22 ++- moveit_core/robot_state/src/conversions.cpp | 53 ++++--- moveit_core/robot_state/src/robot_state.cpp | 75 +++++----- moveit_core/robot_trajectory/CMakeLists.txt | 1 + .../robot_trajectory/src/robot_trajectory.cpp | 14 +- .../test/test_robot_trajectory.cpp | 2 + .../src/ruckig_traj_smoothing.cpp | 30 ++-- .../time_optimal_trajectory_generation.cpp | 80 +++++----- moveit_core/transforms/CMakeLists.txt | 2 +- moveit_core/transforms/src/transforms.cpp | 21 ++- moveit_core/utils/test/CMakeLists.txt | 4 +- .../utils/test/logger_from_child_dut.cpp | 19 ++- .../utils/test/logger_from_only_child_dut.cpp | 16 +- 64 files changed, 956 insertions(+), 647 deletions(-) diff --git a/doc/MIGRATION_GUIDE.md b/doc/MIGRATION_GUIDE.md index f528c173f7..0ef76827b3 100644 --- a/doc/MIGRATION_GUIDE.md +++ b/doc/MIGRATION_GUIDE.md @@ -38,6 +38,20 @@ Once you have a child logger you can use it in logging macros: RCLCPP_INFO(logger_, "Very important info message"); ``` +In some files you'll find the creation of a static logger for the file like this. +Note that this is different from the previous file level static variables because the logger is not initialized until the function is called the first time. +This enables us to set the global node logger before this is called. +```C++ +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("moveit_collision_detection"); + return logger; +} +} // namespace +``` + ### Logger naming convention Migrating the loggers is a good opportunity to make logger names more consistent. diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index e6b7594fb5..70aa54d263 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -35,6 +35,7 @@ set_target_properties(moveit_collision_detection PROPERTIES VERSION ${${PROJECT_ target_link_libraries(moveit_collision_detection moveit_robot_state + moveit_utils ) # unit tests diff --git a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index cf6ee78585..5853a74053 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -38,103 +38,112 @@ #include #include #include +#include -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_world_allvalid"); +namespace collision_detection +{ +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_world_allvalid"); + return logger; +} +} // namespace -const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); +const std::string CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); -collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, - double padding, double scale) +CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding, + double scale) : CollisionEnv(robot_model, padding, scale) { } -collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, - const WorldPtr& world, double padding, double scale) +CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, + double padding, double scale) : CollisionEnv(robot_model, world, padding, scale) { } -collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world) +CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world) : CollisionEnv(other, world) { } -void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& /*state*/) const +void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& /*state*/) const { res.collision = false; if (req.verbose) - RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed."); + RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& /*state*/, - const AllowedCollisionMatrix& /*acm*/) const +void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& /*state*/, + const AllowedCollisionMatrix& /*acm*/) const { res.collision = false; if (req.verbose) - RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed."); + RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& /*state1*/, - const moveit::core::RobotState& /*state2*/) const +void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/) const { res.collision = false; if (req.verbose) - RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed."); + RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& /*state1*/, - const moveit::core::RobotState& /*state2*/, - const AllowedCollisionMatrix& /*acm*/) const +void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/, + const AllowedCollisionMatrix& /*acm*/) const { res.collision = false; if (req.verbose) - RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed."); + RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_detection::DistanceRequest& /*req*/, - collision_detection::DistanceResult& res, - const moveit::core::RobotState& /*state*/) const +void CollisionEnvAllValid::distanceRobot(const DistanceRequest& /*req*/, DistanceResult& res, + const moveit::core::RobotState& /*state*/) const { res.collision = false; } -double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/) const +double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/) const { return 0.0; } -double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/, - const AllowedCollisionMatrix& /*acm*/) const +double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/, + const AllowedCollisionMatrix& /*acm*/) const { return 0.0; } -void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& /*state*/) const +void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& /*state*/) const { res.collision = false; if (req.verbose) - RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed."); + RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& /*state*/, - const AllowedCollisionMatrix& /*acm*/) const +void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& /*state*/, + const AllowedCollisionMatrix& /*acm*/) const { res.collision = false; if (req.verbose) - RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed."); + RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionEnvAllValid::distanceSelf(const collision_detection::DistanceRequest& /*req*/, - collision_detection::DistanceResult& res, - const moveit::core::RobotState& /*state*/) const +void CollisionEnvAllValid::distanceSelf(const DistanceRequest& /*req*/, DistanceResult& res, + const moveit::core::RobotState& /*state*/) const { res.collision = false; } + +} // namespace collision_detection diff --git a/moveit_core/collision_detection/src/collision_common.cpp b/moveit_core/collision_detection/src/collision_common.cpp index c8dacde8a4..8fab70e28d 100644 --- a/moveit_core/collision_detection/src/collision_common.cpp +++ b/moveit_core/collision_detection/src/collision_common.cpp @@ -36,12 +36,21 @@ #include #include #include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_common"); constexpr size_t LOG_THROTTLE_PERIOD{ 5 }; namespace collision_detection { +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_common"); + return logger; +} +} // namespace + void CollisionResult::print() const { rclcpp::Clock clock; @@ -49,16 +58,16 @@ void CollisionResult::print() const { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD, + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD, "Objects in collision (printing 1st of " << contacts.size() << " pairs): " << contacts.begin()->first.first << ", " << contacts.begin()->first.second); // Log all collisions at the debug level - RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD, "Objects in collision:"); + RCLCPP_DEBUG_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD, "Objects in collision:"); for (const auto& contact : contacts) { - RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD, + RCLCPP_DEBUG_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD, "\t" << contact.first.first << ", " << contact.first.second); } #pragma GCC diagnostic pop diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp index bf5955a0c1..4299e25167 100644 --- a/moveit_core/collision_detection/src/collision_env.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -38,20 +38,27 @@ #include #include #include +#include -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_robot"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_env"); + return logger; +} +} // namespace static inline bool validateScale(const double scale) { if (scale < std::numeric_limits::epsilon()) { - RCLCPP_ERROR(LOGGER, "Scale must be positive"); + RCLCPP_ERROR(getLogger(), "Scale must be positive"); return false; } if (scale > std::numeric_limits::max()) { - RCLCPP_ERROR(LOGGER, "Scale must be finite"); + RCLCPP_ERROR(getLogger(), "Scale must be finite"); return false; } return true; @@ -61,12 +68,12 @@ static inline bool validatePadding(const double padding) { if (padding < 0.0) { - RCLCPP_ERROR(LOGGER, "Padding cannot be negative"); + RCLCPP_ERROR(getLogger(), "Padding cannot be negative"); return false; } if (padding > std::numeric_limits::max()) { - RCLCPP_ERROR(LOGGER, "Padding must be finite"); + RCLCPP_ERROR(getLogger(), "Padding must be finite"); return false; } return true; diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 034f3bf537..de4065a6b5 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -39,11 +39,18 @@ #include #include #include +#include namespace collision_detection { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_matrix"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_matrix"); + return logger; +} +} // namespace AllowedCollisionMatrix::AllowedCollisionMatrix() { @@ -76,7 +83,8 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo if (msg.entry_names.size() != msg.entry_values.size() || msg.default_entry_names.size() != msg.default_entry_values.size()) { - RCLCPP_ERROR(LOGGER, "The number of links does not match the number of entries in AllowedCollisionMatrix message"); + RCLCPP_ERROR(getLogger(), + "The number of links does not match the number of entries in AllowedCollisionMatrix message"); return; } for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i) @@ -88,7 +96,7 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo { if (msg.entry_values[i].enabled.size() != msg.entry_names.size()) { - RCLCPP_ERROR(LOGGER, "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message", + RCLCPP_ERROR(getLogger(), "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message", msg.entry_names[i].c_str()); return; } diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index 3840497943..b365ffbee8 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -43,9 +43,16 @@ #include #include #include +#include -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_octomap_filter"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_octomap_filter"); + return logger; +} +} // namespace // forward declarations bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value, @@ -64,12 +71,12 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec { if (!object) { - RCLCPP_ERROR(LOGGER, "No valid Object passed in, cannot refine Normals!"); + RCLCPP_ERROR(getLogger(), "No valid Object passed in, cannot refine Normals!"); return 0; } if (res.contact_count < 1) { - RCLCPP_WARN(LOGGER, "There do not appear to be any contacts, so there is nothing to refine!"); + RCLCPP_WARN(getLogger(), "There do not appear to be any contacts, so there is nothing to refine!"); return 0; } @@ -129,16 +136,16 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec { count++; node_centers.push_back(pt); - // RCLCPP_INFO(LOGGER, "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]", + // RCLCPP_INFO(getLogger(), "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]", // count, prob, pt.x(), pt.y(), pt.z()); } } - // RCLCPP_INFO(LOGGER, "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells + // RCLCPP_INFO(getLogger(), "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); - // RCLCPP_ERROR(LOGGER, "bad stuff in collision_octomap_filter.cpp; need to port octomap + // RCLCPP_ERROR(getLogger(), "bad stuff in collision_octomap_filter.cpp; need to port octomap // call for groovy"); octomath::Vector3 n; @@ -151,7 +158,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec if (divergence > allowed_angle_divergence) { modified++; - // RCLCPP_INFO(LOGGER, "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f, + // RCLCPP_INFO(getLogger(), "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()); @@ -268,7 +275,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_mu } else { - RCLCPP_ERROR(LOGGER, "This should not be called!"); + RCLCPP_ERROR(getLogger(), "This should not be called!"); } double f_val = 0; @@ -294,7 +301,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_mu } else { - RCLCPP_ERROR(LOGGER, "This should not be called!"); + RCLCPP_ERROR(getLogger(), "This should not be called!"); const double r_scaled = r / r; // TODO still need to address the scaling... f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1); diff --git a/moveit_core/collision_detection/src/collision_plugin_cache.cpp b/moveit_core/collision_detection/src/collision_plugin_cache.cpp index ff36bf04a6..d652104357 100644 --- a/moveit_core/collision_detection/src/collision_plugin_cache.cpp +++ b/moveit_core/collision_detection/src/collision_plugin_cache.cpp @@ -37,11 +37,19 @@ #include #include #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_detection"); +#include namespace collision_detection { +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_plugin_cache"); + return logger; +} +} // namespace + class CollisionPluginCache::CollisionPluginCacheImpl { public: @@ -54,7 +62,7 @@ class CollisionPluginCache::CollisionPluginCacheImpl } catch (pluginlib::PluginlibException& e) { - RCLCPP_ERROR(LOGGER, "Unable to construct collision plugin loader. Error: %s", e.what()); + RCLCPP_ERROR(getLogger(), "Unable to construct collision plugin loader. Error: %s", e.what()); } } @@ -68,7 +76,7 @@ class CollisionPluginCache::CollisionPluginCacheImpl } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, "Exception while loading " << name << ": " << ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), "Exception while loading " << name << ": " << ex.what()); } return plugin; } diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index d3032f52fb..fa4dba35bd 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -38,11 +38,18 @@ #include #include #include +#include namespace collision_detection { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.world"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_world"); + return logger; +} +} // namespace World::World() { @@ -74,7 +81,7 @@ void World::addToObject(const std::string& object_id, const Eigen::Isometry3d& p { if (shapes.size() != shape_poses.size()) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Number of shapes and number of poses do not match. Not adding this object to collision world."); return; } @@ -207,7 +214,7 @@ const Eigen::Isometry3d& World::getGlobalShapeTransform(const std::string& objec } else { - RCLCPP_ERROR_STREAM(LOGGER, "Could not find global shape transform for object " << object_id); + RCLCPP_ERROR_STREAM(getLogger(), "Could not find global shape transform for object " << object_id); static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity(); return IDENTITY_TRANSFORM; } @@ -222,7 +229,7 @@ const EigenSTL::vector_Isometry3d& World::getGlobalShapeTransforms(const std::st } else { - RCLCPP_ERROR_STREAM(LOGGER, "Could not find global shape transforms for object " << object_id); + RCLCPP_ERROR_STREAM(getLogger(), "Could not find global shape transforms for object " << object_id); static const EigenSTL::vector_Isometry3d IDENTITY_TRANSFORM_VECTOR; return IDENTITY_TRANSFORM_VECTOR; } diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index f8a2dc59a7..0572a9183a 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -29,6 +29,7 @@ ament_target_dependencies(moveit_collision_detection_bullet ) target_link_libraries(moveit_collision_detection_bullet moveit_collision_detection + moveit_utils ) add_library(collision_detector_bullet_plugin SHARED src/collision_detector_bullet_plugin_loader.cpp) @@ -47,6 +48,7 @@ ament_target_dependencies(collision_detector_bullet_plugin target_link_libraries(collision_detector_bullet_plugin moveit_collision_detection_bullet moveit_planning_scene + moveit_utils ) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h index ac5e470cfa..9a9f4859ea 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h @@ -44,4 +44,6 @@ shapes::ShapePtr constructShape(const urdf::Geometry* geom); Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose); +rclcpp::Logger getLogger(); + } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp index 7aa87d26de..e7153a2b16 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -37,8 +37,7 @@ #include #include #include - -static const rclcpp::Logger BULLET_LOGGER = rclcpp::get_logger("collision_detection.bullet"); +#include namespace collision_detection_bullet { @@ -122,8 +121,8 @@ void BulletCastBVHManager::setCastCollisionObjectsTransform(const std::string& n } else { - RCLCPP_ERROR_STREAM(BULLET_LOGGER, "I can only continuous collision check convex shapes and " - "compound shapes made of convex shapes"); + RCLCPP_ERROR_STREAM(getLogger(), "I can only continuous collision check convex shapes and " + "compound shapes made of convex shapes"); throw std::runtime_error( "I can only continuous collision check convex shapes and compound shapes made of convex shapes"); } @@ -142,7 +141,7 @@ void BulletCastBVHManager::contactTest(collision_detection::CollisionResult& col broadphase_->calculateOverlappingPairs(dispatcher_.get()); btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Number overlapping candidates " << pair_cache->getNumOverlappingPairs()); + RCLCPP_DEBUG_STREAM(getLogger(), "Number overlapping candidates " << pair_cache->getNumOverlappingPairs()); BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, false, true); TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp index 0656347422..29f56f3b3b 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -35,8 +35,7 @@ #include #include - -static const rclcpp::Logger BULLET_LOGGER = rclcpp::get_logger("collision_detection.bullet"); +#include namespace collision_detection_bullet { @@ -71,14 +70,14 @@ void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& broadphase_->calculateOverlappingPairs(dispatcher_.get()); btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Num overlapping candidates " << pair_cache->getNumOverlappingPairs()); + RCLCPP_DEBUG_STREAM(getLogger(), "Num overlapping candidates " << pair_cache->getNumOverlappingPairs()); BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, self); TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, (collisions.collision ? "In" : "No") - << " collision with " << collisions.contact_count << " collisions"); + RCLCPP_DEBUG_STREAM(getLogger(), (collisions.collision ? "In" : "No") + << " collision with " << collisions.contact_count << " collisions"); } void BulletDiscreteBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp index 34ef0c6041..3fe31c3824 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -42,8 +42,7 @@ #include #include #include - -static const rclcpp::Logger BULLET_LOGGER = rclcpp::get_logger("collision_detection.bullet"); +#include namespace collision_detection_bullet { @@ -58,27 +57,26 @@ bool acmCheck(const std::string& body_1, const std::string& body_2, { if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, + RCLCPP_DEBUG_STREAM(getLogger(), "Not allowed entry in ACM found, collision check between " << body_1 << " and " << body_2); return false; } else { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, + RCLCPP_DEBUG_STREAM(getLogger(), "Entry in ACM found, skipping collision check as allowed " << body_1 << " and " << body_2); return true; } } else { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, - "No entry in ACM found, collision check between " << body_1 << " and " << body_2); + RCLCPP_DEBUG_STREAM(getLogger(), "No entry in ACM found, collision check between " << body_1 << " and " << body_2); return false; } } else { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "No ACM, collision check between " << body_1 << " and " << body_2); + RCLCPP_DEBUG_STREAM(getLogger(), "No ACM, collision check between " << body_1 << " and " << body_2); return false; } } @@ -189,13 +187,13 @@ btCollisionShape* createShapePrimitive(const shapes::Mesh* geom, const Collision } default: { - RCLCPP_ERROR(BULLET_LOGGER, "This bullet shape type (%d) is not supported for geometry meshs", + RCLCPP_ERROR(getLogger(), "This bullet shape type (%d) is not supported for geometry meshs", static_cast(collision_object_type)); return nullptr; } } } - RCLCPP_ERROR(BULLET_LOGGER, "The mesh is empty!"); + RCLCPP_ERROR(getLogger(), "The mesh is empty!"); return nullptr; } @@ -262,7 +260,7 @@ btCollisionShape* createShapePrimitive(const shapes::OcTree* geom, const Collisi } default: { - RCLCPP_ERROR(BULLET_LOGGER, "This bullet shape type (%d) is not supported for geometry octree", + RCLCPP_ERROR(getLogger(), "This bullet shape type (%d) is not supported for geometry octree", static_cast(collision_object_type)); return nullptr; } @@ -288,8 +286,8 @@ void updateCollisionObjectFilters(const std::vector& active, Collis cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collisionFilterGroup; cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collisionFilterMask; } - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "COW " << cow.getName() << " group " << cow.m_collisionFilterGroup << " mask " - << cow.m_collisionFilterMask); + RCLCPP_DEBUG_STREAM(getLogger(), "COW " << cow.getName() << " group " << cow.m_collisionFilterGroup << " mask " + << cow.m_collisionFilterMask); } CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow) @@ -364,7 +362,7 @@ CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPt } else { - RCLCPP_ERROR_STREAM(BULLET_LOGGER, + RCLCPP_ERROR_STREAM(getLogger(), "I can only collision check convex shapes and compound shapes made of convex shapes"); throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes"); } @@ -378,7 +376,7 @@ CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPt } else { - RCLCPP_ERROR_STREAM(BULLET_LOGGER, + RCLCPP_ERROR_STREAM(getLogger(), "I can only collision check convex shapes and compound shapes made of convex shapes"); throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes"); } @@ -390,7 +388,7 @@ void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, const std::unique_ptr& broadphase, const std::unique_ptr& dispatcher) { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Added " << cow->getName() << " to broadphase"); + RCLCPP_DEBUG_STREAM(getLogger(), "Added " << cow->getName() << " to broadphase"); btVector3 aabb_min, aabb_max; cow->getAABB(aabb_min, aabb_max); @@ -430,7 +428,7 @@ btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, } default: { - RCLCPP_ERROR(BULLET_LOGGER, "This geometric shape type (%d) is not supported using BULLET yet", + RCLCPP_ERROR(getLogger(), "This geometric shape type (%d) is not supported using BULLET yet", static_cast(geom->type)); return nullptr; } @@ -475,7 +473,7 @@ bool BroadphaseFilterCallback::needBroadphaseCollision(btBroadphaseProxy* proxy0 return false; } - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Broadphase pass " << cow0->getName() << " vs " << cow1->getName()); + RCLCPP_DEBUG_STREAM(getLogger(), "Broadphase pass " << cow0->getName() << " vs " << cow1->getName()); return true; } @@ -486,7 +484,7 @@ btScalar BroadphaseContactResultCallback::addSingleResult(btManifoldPoint& cp, { if (cp.m_distance1 > static_cast(contact_distance_)) { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Not close enough for collision with " << cp.m_distance1); + RCLCPP_DEBUG_STREAM(getLogger(), "Not close enough for collision with " << cp.m_distance1); return 0; } @@ -515,7 +513,7 @@ bool TesseractCollisionPairCallback::processOverlap(btBroadphasePair& pair) std::pair pair_names{ cow0->getName(), cow1->getName() }; if (results_callback_.needsCollision(cow0, cow1)) { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Processing " << cow0->getName() << " vs " << cow1->getName()); + RCLCPP_DEBUG_STREAM(getLogger(), "Processing " << cow0->getName() << " vs " << cow1->getName()); btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); @@ -536,7 +534,7 @@ bool TesseractCollisionPairCallback::processOverlap(btBroadphasePair& pair) } else { - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Not processing " << cow0->getName() << " vs " << cow1->getName()); + RCLCPP_DEBUG_STREAM(getLogger(), "Not processing " << cow0->getName() << " vs " << cow1->getName()); } return false; } diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp index 5d1b046b23..4da803358e 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp @@ -37,8 +37,7 @@ #include #include - -static const rclcpp::Logger BULLET_LOGGER = rclcpp::get_logger("collision_detection.bullet"); +#include namespace collision_detection_bullet { @@ -54,7 +53,7 @@ collision_detection::Contact* processResult(ContactTestData& cdata, collision_de } } - RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Contact btw " << key.first << " and " << key.second << " dist: " << contact.depth); + RCLCPP_DEBUG_STREAM(getLogger(), "Contact btw " << key.first << " and " << key.second << " dist: " << contact.depth); // case if pair hasn't a contact yet if (!found) { @@ -139,7 +138,7 @@ int createConvexHull(AlignedVector& vertices, std::vector& static_cast(shrinkClamp)); if (val < 0) { - RCLCPP_ERROR(BULLET_LOGGER, "Failed to create convex hull"); + RCLCPP_ERROR(getLogger(), "Failed to create convex hull"); return -1; } diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp index 766aae6315..8cdc0c1dec 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp @@ -35,8 +35,7 @@ #include #include - -const rclcpp::Logger BULLET_LOGGER = rclcpp::get_logger("collision_detection.bullet"); +#include namespace collision_detection_bullet { @@ -98,7 +97,7 @@ shapes::ShapePtr constructShape(const urdf::Geometry* geom) } break; default: - RCLCPP_ERROR(BULLET_LOGGER, "Unknown geometry type: %d", static_cast(geom->type)); + RCLCPP_ERROR(getLogger(), "Unknown geometry type: %d", static_cast(geom->type)); break; } @@ -114,4 +113,10 @@ Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose) return result; } +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_detection_bullet"); + return logger; +} + } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index ad1d8cf7d3..873dfb0a21 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -45,10 +45,11 @@ namespace collision_detection { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.core.collision_detection.bullet"); const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); const double MAX_DISTANCE_MARGIN = 99; +using collision_detection_bullet::getLogger; + CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding, double scale) : CollisionEnv(model, padding, scale) { @@ -240,13 +241,13 @@ void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& re void CollisionEnvBullet::distanceSelf(const DistanceRequest& /*req*/, DistanceResult& /*res*/, const moveit::core::RobotState& /*state*/) const { - RCLCPP_INFO(LOGGER, "distanceSelf is not implemented for Bullet."); + RCLCPP_INFO(getLogger(), "distanceSelf is not implemented for Bullet."); } void CollisionEnvBullet::distanceRobot(const DistanceRequest& /*req*/, DistanceResult& /*res*/, const moveit::core::RobotState& /*state*/) const { - RCLCPP_INFO(LOGGER, "distanceRobot is not implemented for Bullet."); + RCLCPP_INFO(getLogger(), "distanceRobot is not implemented for Bullet."); } void CollisionEnvBullet::addToManager(const World::Object* obj) @@ -354,7 +355,7 @@ void CollisionEnvBullet::addAttachedObjects(const moveit::core::RobotState& stat } catch (std::exception&) { - RCLCPP_ERROR_STREAM(LOGGER, "Not adding " << body->getName() << " due to bad arguments."); + RCLCPP_ERROR_STREAM(getLogger(), "Not adding " << body->getName() << " due to bad arguments."); } } } @@ -369,7 +370,7 @@ void CollisionEnvBullet::updatedPaddingOrScaling(const std::vector& } else { - RCLCPP_ERROR(LOGGER, "Updating padding or scaling for unknown link: '%s'", link.c_str()); + RCLCPP_ERROR(getLogger(), "Updating padding or scaling for unknown link: '%s'", link.c_str()); } } } @@ -443,7 +444,7 @@ void CollisionEnvBullet::addLinkAsCollisionObject(const urdf::LinkSharedPtr& lin } catch (std::exception&) { - RCLCPP_ERROR_STREAM(LOGGER, "Not adding " << link->name << " due to bad arguments."); + RCLCPP_ERROR_STREAM(getLogger(), "Not adding " << link->name << " due to bad arguments."); } } } diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index ac1179c93c..e449297e0f 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -20,6 +20,7 @@ ament_target_dependencies(moveit_collision_detection_fcl ) target_link_libraries(moveit_collision_detection_fcl moveit_collision_detection + moveit_utils fcl ) @@ -35,6 +36,7 @@ ament_target_dependencies(collision_detector_fcl_plugin target_link_libraries(collision_detector_fcl_plugin moveit_collision_detection_fcl moveit_planning_scene + moveit_utils ) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 658a84c119..e4ea209d2e 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) #include @@ -55,8 +56,14 @@ namespace collision_detection { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection_fcl.collision_common"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("moveit_collision_detection_fcl"); + return logger; +} +} // namespace bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data) { @@ -103,7 +110,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi always_allow_collision = true; if (cdata->req_->verbose) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. " "No contacts are computed.", cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), @@ -115,7 +122,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), dcf); if (cdata->req_->verbose) { - RCLCPP_DEBUG(LOGGER, "Collision between '%s' and '%s' is conditionally allowed", cd1->getID().c_str(), + RCLCPP_DEBUG(getLogger(), "Collision between '%s' and '%s' is conditionally allowed", cd1->getID().c_str(), cd2->getID().c_str()); } } @@ -131,7 +138,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi always_allow_collision = true; if (cdata->req_->verbose) { - RCLCPP_DEBUG(LOGGER, "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.", + RCLCPP_DEBUG(getLogger(), "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.", cd1->getID().c_str(), cd2->getID().c_str()); } } @@ -144,7 +151,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi always_allow_collision = true; if (cdata->req_->verbose) { - RCLCPP_DEBUG(LOGGER, "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.", + RCLCPP_DEBUG(getLogger(), "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.", cd2->getID().c_str(), cd1->getID().c_str()); } } @@ -161,7 +168,8 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi return false; if (cdata->req_->verbose) - RCLCPP_DEBUG(LOGGER, "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); + RCLCPP_DEBUG(getLogger(), "Actually checking collisions between %s and %s", cd1->getID().c_str(), + cd2->getID().c_str()); // see if we need to compute a contact std::size_t want_contact_count{ 0 }; @@ -203,7 +211,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi { if (cdata->req_->verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Found %d contacts between '%s' and '%s'. " "These contacts will be evaluated to check if they are accepted or not", num_contacts, cd1->getID().c_str(), cd2->getID().c_str()); @@ -226,13 +234,13 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi cdata->res_->contact_count++; if (cdata->req_->verbose) { - RCLCPP_INFO(LOGGER, "Found unacceptable contact between '%s' and '%s'. Contact was stored.", + RCLCPP_INFO(getLogger(), "Found unacceptable contact between '%s' and '%s'. Contact was stored.", cd1->getID().c_str(), cd2->getID().c_str()); } } else if (cdata->req_->verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Found unacceptable contact between '%s' (type '%s') and '%s' " "(type '%s'). Contact was stored.", cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), @@ -291,7 +299,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi if (cdata->req_->verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), " "which constitute a collision. %d contacts will be stored", num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), @@ -339,7 +347,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi cdata->res_->collision = true; if (cdata->req_->verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Found a contact between '%s' (type '%s') and '%s' (type '%s'), " "which constitutes a collision. " "Contact information is not stored.", @@ -373,7 +381,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi cdata->done_ = true; if (cdata->req_->verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Collision checking is considered complete (collision was found and %u contacts are stored)", static_cast(cdata->res_->contact_count)); } @@ -385,7 +393,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi cdata->done_ = cdata->req_->is_done(*cdata->res_); if (cdata->done_ && cdata->req_->verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Collision checking is considered complete due to external callback. " "%s was found. %u contacts are stored.", cdata->res_->collision ? "Collision" : "No collision", @@ -424,7 +432,7 @@ struct FCLShapeCache map_.erase(it); it = nit; } - // RCLCPP_DEBUG(LOGGER, "Cleaning up cache for FCL objects that correspond to static + // RCLCPP_DEBUG(getLogger(), "Cleaning up cache for FCL objects that correspond to static // shapes. Cache size // reduced from %u // to %u", from, static_cast(map_.size())); @@ -485,7 +493,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void always_allow_collision = true; if (cdata->req->verbose) { - RCLCPP_DEBUG(LOGGER, "Collision between '%s' and '%s' is always allowed. No distances are computed.", + RCLCPP_DEBUG(getLogger(), "Collision between '%s' and '%s' is always allowed. No distances are computed.", cd1->getID().c_str(), cd2->getID().c_str()); } } @@ -501,7 +509,8 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void always_allow_collision = true; if (cdata->req->verbose) { - RCLCPP_DEBUG(LOGGER, "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", + RCLCPP_DEBUG(getLogger(), + "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", cd1->getID().c_str(), cd2->getID().c_str()); } } @@ -514,7 +523,8 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void always_allow_collision = true; if (cdata->req->verbose) { - RCLCPP_DEBUG(LOGGER, "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", + RCLCPP_DEBUG(getLogger(), + "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", cd2->getID().c_str(), cd1->getID().c_str()); } } @@ -525,7 +535,8 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void return false; } if (cdata->req->verbose) - RCLCPP_DEBUG(LOGGER, "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); + RCLCPP_DEBUG(getLogger(), "Actually checking collisions between %s and %s", cd1->getID().c_str(), + cd2->getID().c_str()); double dist_threshold = cdata->req->distance_threshold; @@ -744,7 +755,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, { if (cache_it->second->collision_geometry_data_->ptr.raw == data) { - // RCLCPP_DEBUG(LOGGER, "Collision data structures for object %s retrieved from + // RCLCPP_DEBUG(getLogger(), "Collision data structures for object %s retrieved from // cache.", // cache_it->second->collision_geometry_data_->getID().c_str()); return cache_it->second; @@ -752,7 +763,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, else if (cache_it->second.unique()) { const_cast(cache_it->second.get())->updateCollisionGeometryData(data, shape_index, false); - // RCLCPP_DEBUG(LOGGER, "Collision data structures for object %s retrieved from + // RCLCPP_DEBUG(getLogger(), "Collision data structures for object %s retrieved from // cache after updating // the source // object.", cache_it->second->collision_geometry_data_->getID().c_str()); @@ -781,7 +792,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it const_cast(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true); - // RCLCPP_DEBUG(LOGGER, "Collision data structures for attached body %s retrieved + // RCLCPP_DEBUG(getLogger(), "Collision data structures for attached body %s retrieved // from the cache for // world objects.", // obj_cache->collision_geometry_data_->getID().c_str()); @@ -815,7 +826,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it const_cast(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true); - // RCLCPP_DEBUG(LOGGER, "Collision data structures for world object %s retrieved + // RCLCPP_DEBUG(getLogger(), "Collision data structures for world object %s retrieved // from the cache for // attached // bodies.", @@ -895,7 +906,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, } break; default: - RCLCPP_ERROR(LOGGER, "This shape type (%d) is not supported using FCL yet", static_cast(shape->type)); + RCLCPP_ERROR(getLogger(), "This shape type (%d) is not supported using FCL yet", static_cast(shape->type)); cg_g = nullptr; } diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 4a8487cb58..56e2715404 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) #include @@ -48,11 +49,16 @@ namespace collision_detection { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection_fcl.collision_env_fcl"); const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); namespace { +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("moveit_collision_detection_fcl"); + return logger; +} + // Check whether this FCL version supports the requested computations void checkFCLCapabilities(const DistanceRequest& req) { @@ -63,7 +69,7 @@ void checkFCLCapabilities(const DistanceRequest& req) // https://github.com/flexible-collision-library/fcl/issues/171, // https://github.com/flexible-collision-library/fcl/pull/288 rclcpp::Clock steady_clock(RCL_STEADY_TIME); - RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 2000, + RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000, "You requested a distance check with enable_nearest_points=true, " "but the FCL version MoveIt was compiled against (%d.%d.%d) " "is known to return bogus nearest points. Please update your FCL " @@ -103,7 +109,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, std::make_shared(link_geometry->collision_geometry_)); } else - RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); + RCLCPP_ERROR(getLogger(), "Unable to construct collision geometry for link '%s'", link->getName().c_str()); } } @@ -141,7 +147,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, robot_fcl_objs_[index] = std::make_shared(g->collision_geometry_); } else - RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); + RCLCPP_ERROR(getLogger(), "Unable to construct collision geometry for link '%s'", link->getName().c_str()); } } @@ -305,7 +311,7 @@ void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& /*req*/, Colli const moveit::core::RobotState& /*state1*/, const moveit::core::RobotState& /*state2*/) const { - RCLCPP_ERROR(LOGGER, "Continuous collision not implemented"); + RCLCPP_ERROR(getLogger(), "Continuous collision not implemented"); } void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/, @@ -313,7 +319,7 @@ void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& /*req*/, Colli const moveit::core::RobotState& /*state2*/, const AllowedCollisionMatrix& /*acm*/) const { - RCLCPP_ERROR(LOGGER, "Not implemented"); + RCLCPP_ERROR(getLogger(), "Not implemented"); } void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, @@ -466,7 +472,7 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& li } } else - RCLCPP_ERROR(LOGGER, "Updating padding or scaling for unknown link: '%s'", link.c_str()); + RCLCPP_ERROR(getLogger(), "Updating padding or scaling for unknown link: '%s'", link.c_str()); } } diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 3e71c31651..34fa321d0d 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -56,6 +56,7 @@ #include #include #include +#include namespace collision_detection { @@ -372,7 +373,7 @@ class PosedBodySphereDecompositionVector public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PosedBodySphereDecompositionVector() + PosedBodySphereDecompositionVector() : logger_(moveit::makeChildLogger("posed_body_sphere_decomposition_vector")) { } @@ -411,7 +412,7 @@ class PosedBodySphereDecompositionVector { if (i >= decomp_vector_.size()) { - RCLCPP_INFO(LOGGER, "No body decomposition"); + RCLCPP_INFO(logger_, "No body decomposition"); return empty_ptr_; } return decomp_vector_[i]; @@ -421,7 +422,7 @@ class PosedBodySphereDecompositionVector { if (ind >= decomp_vector_.size()) { - RCLCPP_WARN(LOGGER, "Can't update pose"); + RCLCPP_WARN(logger_, "Can't update pose"); return; } decomp_vector_[ind]->updatePose(pose); @@ -432,7 +433,7 @@ class PosedBodySphereDecompositionVector } private: - static const rclcpp::Logger LOGGER; + rclcpp::Logger logger_; PosedBodySphereDecompositionConstPtr empty_ptr_; std::vector decomp_vector_; std::vector collision_spheres_; @@ -446,7 +447,7 @@ class PosedBodyPointDecompositionVector public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PosedBodyPointDecompositionVector() + PosedBodyPointDecompositionVector() : logger_(moveit::makeChildLogger("posed_body_point_decomposition_vector")) { } @@ -474,7 +475,7 @@ class PosedBodyPointDecompositionVector { if (i >= decomp_vector_.size()) { - RCLCPP_INFO(LOGGER, "No body decomposition"); + RCLCPP_INFO(logger_, "No body decomposition"); return empty_ptr_; } return decomp_vector_[i]; @@ -488,13 +489,13 @@ class PosedBodyPointDecompositionVector } else { - RCLCPP_WARN(LOGGER, "Can't update pose"); + RCLCPP_WARN(logger_, "Can't update pose"); return; } } protected: - static const rclcpp::Logger LOGGER; + rclcpp::Logger logger_; private: PosedBodyPointDecompositionPtr empty_ptr_; diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index a4afd83ef3..f971f5a442 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -120,7 +120,7 @@ class CollisionEnvDistanceField : public CollisionEnv void distanceSelf(const DistanceRequest& /* req */, DistanceResult& /* res */, const moveit::core::RobotState& /* state */) const override { - RCLCPP_ERROR(LOGGER, "Not implemented"); + RCLCPP_ERROR(logger_, "Not implemented"); } DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const @@ -195,7 +195,7 @@ class CollisionEnvDistanceField : public CollisionEnv void distanceRobot(const DistanceRequest& /* req */, DistanceResult& /* res */, const moveit::core::RobotState& /* state */) const override { - RCLCPP_ERROR(LOGGER, "Not implemented"); + RCLCPP_ERROR(logger_, "Not implemented"); } void setWorld(const WorldPtr& world) override; @@ -283,8 +283,7 @@ class CollisionEnvDistanceField : public CollisionEnv void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); - // Logger - static const rclcpp::Logger LOGGER; + rclcpp::Logger logger_; Eigen::Vector3d size_; Eigen::Vector3d origin_; diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp index 442aaad191..7e79deb2c7 100644 --- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp @@ -42,11 +42,19 @@ #include #include #include +#include namespace collision_detection { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_collision_distance_field.collision_common_distance_field"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("collision_common_distance_field"); + return logger; +} +} // namespace + struct BodyDecompositionCache { using Comperator = std::owner_less; @@ -199,7 +207,7 @@ void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& g const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]); if (!att) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Attached body '%s' was not found, skipping sphere " "decomposition visualization", gsr->dfce_->attached_body_names_[i].c_str()); @@ -208,7 +216,7 @@ void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& g if (gsr->attached_body_decompositions_[i]->getSize() != att->getShapes().size()) { - RCLCPP_WARN(LOGGER, "Attached body size discrepancy"); + RCLCPP_WARN(getLogger(), "Attached body size discrepancy"); continue; } diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index f89864803d..4cdc53fa7a 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -622,5 +622,3 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con } } } // namespace collision_detection -const rclcpp::Logger collision_detection::PosedBodyPointDecompositionVector::LOGGER = collision_detection::LOGGER; -const rclcpp::Logger collision_detection::PosedBodySphereDecompositionVector::LOGGER = collision_detection::LOGGER; diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 89135537a7..92bffb492c 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -48,11 +48,10 @@ #include #include #include +#include namespace collision_detection { -const rclcpp::Logger CollisionEnvDistanceField::LOGGER = - rclcpp::get_logger("moveit_collision_distance_field.collision_robot_distance_field"); const double EPSILON = 0.001f; const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD"); @@ -62,7 +61,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double /*padding*/, double /*scale*/) - : CollisionEnv(robot_model) + : CollisionEnv(robot_model), logger_(moveit::makeChildLogger("collision_robot_distance_field")) { initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); @@ -81,7 +80,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) - : CollisionEnv(robot_model, world, padding, scale) + : CollisionEnv(robot_model, world, padding, scale), logger_(moveit::makeChildLogger("collision_robot_distance_field")) { initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); @@ -96,7 +95,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( } CollisionEnvDistanceField::CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world) - : CollisionEnv(other, world) + : CollisionEnv(other, world), logger_(other.logger_) { size_ = other.size_; origin_ = other.origin_; @@ -195,7 +194,7 @@ void CollisionEnvDistanceField::checkSelfCollisionHelper(const collision_detecti { getIntraGroupCollisions(req, res, gsr); if (res.collision) - RCLCPP_DEBUG(LOGGER, "Intra Group Collision found"); + RCLCPP_DEBUG(logger_, "Intra Group Collision found"); } } @@ -207,13 +206,13 @@ CollisionEnvDistanceField::getDistanceFieldCacheEntry(const std::string& group_n DistanceFieldCacheEntryConstPtr ret; if (!distance_field_cache_entry_) { - RCLCPP_DEBUG(LOGGER, " No current Distance field cache entry"); + RCLCPP_DEBUG(logger_, " No current Distance field cache entry"); return ret; } DistanceFieldCacheEntryConstPtr cur = distance_field_cache_entry_; if (group_name != cur->group_name_) { - RCLCPP_DEBUG(LOGGER, "No cache entry as group name changed from %s to %s", cur->group_name_.c_str(), + RCLCPP_DEBUG(logger_, "No cache entry as group name changed from %s to %s", cur->group_name_.c_str(), group_name.c_str()); return ret; } @@ -226,7 +225,7 @@ CollisionEnvDistanceField::getDistanceFieldCacheEntry(const std::string& group_n } else if (acm && !compareCacheEntryToAllowedCollisionMatrix(cur, *acm)) { - RCLCPP_DEBUG(LOGGER, "Regenerating distance field as some relevant part of the acm changed"); + RCLCPP_DEBUG(logger_, "Regenerating distance field as some relevant part of the acm changed"); return ret; } return cur; @@ -265,8 +264,8 @@ void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::Co { if (gsr) { - RCLCPP_WARN(LOGGER, "Shouldn't be calling this function with initialized gsr - ACM " - "will be ignored"); + RCLCPP_WARN(logger_, "Shouldn't be calling this function with initialized gsr - ACM " + "will be ignored"); } checkSelfCollisionHelper(req, res, state, &acm, gsr); } @@ -321,7 +320,7 @@ bool CollisionEnvDistanceField::getSelfCollisions(const collision_detection::Col con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()]; } - RCLCPP_DEBUG(LOGGER, "Self collision detected for link %s ", con.body_name_1.c_str()); + RCLCPP_DEBUG(logger_, "Self collision detected for link %s ", con.body_name_1.c_str()); con.body_type_2 = collision_detection::BodyTypes::ROBOT_LINK; con.body_name_2 = "self"; @@ -343,7 +342,7 @@ bool CollisionEnvDistanceField::getSelfCollisions(const collision_detection::Col *sphere_centers_1, max_propogation_distance_, collision_tolerance_); if (coll) { - RCLCPP_DEBUG(LOGGER, "Link %s in self collision", gsr->dfce_->link_names_[i].c_str()); + RCLCPP_DEBUG(logger_, "Link %s in self collision", gsr->dfce_->link_names_[i].c_str()); res.collision = true; return true; } @@ -582,14 +581,14 @@ bool CollisionEnvDistanceField::getIntraGroupCollisions(const collision_detectio if (dist < (*collision_spheres_1)[k].radius_ + (*collision_spheres_2)[l].radius_) { - // RCLCPP_DEBUG(LOGGER,"Intra-group contact between %s and %s, d = + // RCLCPP_DEBUG(logger_,"Intra-group contact between %s and %s, d = // %f < r1 = %f + r2 = %f", name_1.c_str(), // name_2.c_str(), // dist ,(*collision_spheres_1)[k].radius_ // ,(*collision_spheres_2)[l].radius_); // Eigen::Vector3d sc1 = (*sphere_centers_1)[k]; // Eigen::Vector3d sc2 = (*sphere_centers_2)[l]; - // RCLCPP_DEBUG(LOGGER,"sphere center 1:[ %f, %f, %f ], sphere + // RCLCPP_DEBUG(logger_,"sphere center 1:[ %f, %f, %f ], sphere // center 2: [%f, %f,%f ], lbdc size = // %i",sc1[0],sc1[1],sc1[2], // sc2[0],sc2[1],sc2[2],int(gsr->link_body_decompositions_.size())); @@ -715,7 +714,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache if (robot_model_->getJointModelGroup(group_name) == nullptr) { - RCLCPP_WARN(LOGGER, "No group %s", group_name.c_str()); + RCLCPP_WARN(logger_, "No group %s", group_name.c_str()); return dfce; } @@ -727,8 +726,8 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache } else { - RCLCPP_WARN(LOGGER, "Allowed Collision Matrix is null, enabling all collisions " - "in the DistanceFieldCacheEntry"); + RCLCPP_WARN(logger_, "Allowed Collision Matrix is null, enabling all collisions " + "in the DistanceFieldCacheEntry"); } // generateAllowedCollisionInformation(dfce); @@ -763,7 +762,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache if (!found) { - RCLCPP_DEBUG(LOGGER, "No link state found for link %s", dfce->link_names_[i].c_str()); + RCLCPP_DEBUG(logger_, "No link state found for link %s", dfce->link_names_[i].c_str()); continue; } @@ -879,7 +878,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache for (const moveit::core::JointModel* child_joint_model : child_joint_models) { updated_map[child_joint_model->getName()] = true; - RCLCPP_DEBUG(LOGGER, "Joint %s has been added to updated list ", child_joint_model->getName().c_str()); + RCLCPP_DEBUG(logger_, "Joint %s has been added to updated list ", child_joint_model->getName().c_str()); } } @@ -891,7 +890,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache if (updated_map.count(state_variable_name) == 0) { dfce->state_check_indices_.push_back(dfce->state_values_.size() - 1); - RCLCPP_DEBUG(LOGGER, "Non-group joint %p will be checked for state changes", state_variable_name.c_str()); + RCLCPP_DEBUG(logger_, "Non-group joint %p will be checked for state changes", state_variable_name.c_str()); } } @@ -899,8 +898,8 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache { if (dfce->distance_field_) { - RCLCPP_DEBUG(LOGGER, "CollisionRobot skipping distance field generation, " - "will use existing one"); + RCLCPP_DEBUG(logger_, "CollisionRobot skipping distance field generation, " + "will use existing one"); } else { @@ -951,7 +950,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache } dfce->distance_field_->addPointsToField(all_points); - RCLCPP_DEBUG(LOGGER, "CollisionRobot distance field has been initialized with %zu points.", all_points.size()); + RCLCPP_DEBUG(logger_, "CollisionRobot distance field has been initialized with %zu points.", all_points.size()); } } return dfce; @@ -964,12 +963,12 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions(double resolution) { if (link_model->getShapes().empty()) { - RCLCPP_WARN(LOGGER, "No collision geometry for link model %s though there should be", + RCLCPP_WARN(logger_, "No collision geometry for link model %s though there should be", link_model->getName().c_str()); continue; } - RCLCPP_DEBUG(LOGGER, "Generating model for %s", link_model->getName().c_str()); + RCLCPP_DEBUG(logger_, "Generating model for %s", link_model->getName().c_str()); BodyDecompositionConstPtr bd = std::make_shared(link_model->getShapes(), link_model->getCollisionOriginTransforms(), resolution, getLinkPadding(link_model->getName())); @@ -1056,7 +1055,7 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions( { if (link_model->getShapes().empty()) { - RCLCPP_WARN(LOGGER, "Skipping model generation for link %s since it contains no geometries", + RCLCPP_WARN(logger_, "Skipping model generation for link %s since it contains no geometries", link_model->getName().c_str()); continue; } @@ -1065,7 +1064,7 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions( std::make_shared(link_model->getShapes(), link_model->getCollisionOriginTransforms(), resolution, getLinkPadding(link_model->getName())); - RCLCPP_DEBUG(LOGGER, "Generated model for %s", link_model->getName().c_str()); + RCLCPP_DEBUG(logger_, "Generated model for %s", link_model->getName().c_str()); if (link_spheres.find(link_model->getName()) != link_spheres.end()) { @@ -1074,7 +1073,7 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions( link_body_decomposition_vector_.push_back(bd); link_body_decomposition_index_map_[link_model->getName()] = link_body_decomposition_vector_.size() - 1; } - RCLCPP_DEBUG(LOGGER, " Finished "); + RCLCPP_DEBUG(logger_, " Finished "); } PosedBodySphereDecompositionPtr @@ -1093,7 +1092,7 @@ CollisionEnvDistanceField::getPosedLinkBodyPointDecomposition(const moveit::core std::map::const_iterator it = link_body_decomposition_index_map_.find(ls->getName()); if (it == link_body_decomposition_index_map_.end()) { - RCLCPP_ERROR(LOGGER, "No link body decomposition for link %s.", ls->getName().c_str()); + RCLCPP_ERROR(logger_, "No link body decomposition for link %s.", ls->getName().c_str()); return ret; } ret = std::make_shared(link_body_decomposition_vector_[it->second]); @@ -1125,14 +1124,14 @@ void CollisionEnvDistanceField::updateGroupStateRepresentationState(const moveit const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]); if (!att) { - RCLCPP_WARN(LOGGER, "Attached body discrepancy"); + RCLCPP_WARN(logger_, "Attached body discrepancy"); continue; } // TODO: This logic for checking attached body count might be incorrect if (gsr->attached_body_decompositions_.size() != att->getShapes().size()) { - RCLCPP_WARN(LOGGER, "Attached body size discrepancy"); + RCLCPP_WARN(logger_, "Attached body size discrepancy"); continue; } @@ -1160,7 +1159,7 @@ void CollisionEnvDistanceField::getGroupStateRepresentation(const DistanceFieldC { if (!dfce->pregenerated_group_state_representation_) { - RCLCPP_DEBUG(LOGGER, "Creating GroupStateRepresentation"); + RCLCPP_DEBUG(logger_, "Creating GroupStateRepresentation"); // unsigned int count = 0; gsr = std::make_shared(); @@ -1183,14 +1182,14 @@ void CollisionEnvDistanceField::getGroupStateRepresentation(const DistanceFieldC link_size = Eigen::Vector3d(diameter, diameter, diameter); link_origin = link_bd->getBoundingSphereCenter() - 0.5 * link_size; - RCLCPP_DEBUG(LOGGER, "Creating PosedDistanceField for link %s with size [%f, %f , %f] and origin %f %f %f ", + RCLCPP_DEBUG(logger_, "Creating PosedDistanceField for link %s with size [%f, %f , %f] and origin %f %f %f ", dfce->link_names_[i].c_str(), link_size.x(), link_size.y(), link_size.z(), link_origin.x(), link_origin.y(), link_origin.z()); gsr->link_distance_fields_.push_back(std::make_shared( link_size, link_origin, resolution_, max_propogation_distance_, use_signed_distance_field_)); gsr->link_distance_fields_.back()->addPointsToField(link_bd->getCollisionPoints()); - RCLCPP_DEBUG(LOGGER, "Created PosedDistanceField for link %s with %zu points", dfce->link_names_[i].c_str(), + RCLCPP_DEBUG(logger_, "Created PosedDistanceField for link %s with %zu points", dfce->link_names_[i].c_str(), link_bd->getCollisionPoints().size()); gsr->link_body_decompositions_.back()->updatePose(state.getFrameTransform(ls->getName())); @@ -1262,7 +1261,7 @@ bool CollisionEnvDistanceField::compareCacheEntryToState(const DistanceFieldCach if (dfce->state_values_.size() != new_state_values.size()) { - RCLCPP_ERROR(LOGGER, " State value size mismatch"); + RCLCPP_ERROR(logger_, " State value size mismatch"); return false; } @@ -1272,7 +1271,7 @@ bool CollisionEnvDistanceField::compareCacheEntryToState(const DistanceFieldCach fabs(dfce->state_values_[dfce->state_check_indices_[i]] - new_state_values[dfce->state_check_indices_[i]]); if (diff > EPSILON) { - RCLCPP_WARN(LOGGER, "State for Variable %s has changed by %f radians", + RCLCPP_WARN(logger_, "State for Variable %s has changed by %f radians", state.getVariableNames()[dfce->state_check_indices_[i]].c_str(), diff); return false; } @@ -1316,7 +1315,7 @@ bool CollisionEnvDistanceField::compareCacheEntryToAllowedCollisionMatrix( { if (dfce->acm_.getSize() != acm.getSize()) { - RCLCPP_DEBUG(LOGGER, "Allowed collision matrix size mismatch"); + RCLCPP_DEBUG(logger_, "Allowed collision matrix size mismatch"); return false; } std::vector attached_bodies; @@ -1504,14 +1503,14 @@ void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& /*re const moveit::core::RobotState& /*state2*/, const AllowedCollisionMatrix& /*acm*/) const { - RCLCPP_ERROR(LOGGER, "Continuous collision checking not implemented"); + RCLCPP_ERROR(logger_, "Continuous collision checking not implemented"); } void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/, const moveit::core::RobotState& /*state1*/, const moveit::core::RobotState& /*state2*/) const { - RCLCPP_ERROR(LOGGER, "Continuous collision checking not implemented"); + RCLCPP_ERROR(logger_, "Continuous collision checking not implemented"); } void CollisionEnvDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& /*res*/, @@ -1724,7 +1723,7 @@ void CollisionEnvDistanceField::notifyObjectChange(const ObjectConstPtr& obj, Wo distance_field_cache_entry_world_->distance_field_->addPointsToField(add_points); } - RCLCPP_DEBUG(LOGGER, "Modifying object %s took %lf s", obj->id_.c_str(), (clock.now() - start_time).seconds()); + RCLCPP_DEBUG(logger_, "Modifying object %s took %lf s", obj->id_.c_str(), (clock.now() - start_time).seconds()); } void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, DistanceFieldCacheEntryWorldPtr& dfce, @@ -1745,7 +1744,7 @@ void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, Dist World::ObjectConstPtr object = getWorld()->getObject(id); if (object) { - RCLCPP_DEBUG(LOGGER, "Updating/Adding Object '%s' with %lu shapes to CollisionEnvDistanceField", + RCLCPP_DEBUG(logger_, "Updating/Adding Object '%s' with %lu shapes to CollisionEnvDistanceField", object->id_.c_str(), object->shapes_.size()); std::vector shape_points; for (unsigned int i{ 0 }; i < object->shapes_.size(); ++i) @@ -1773,7 +1772,7 @@ void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, Dist } else { - RCLCPP_DEBUG(LOGGER, "Removing Object '%s' from CollisionEnvDistanceField", id.c_str()); + RCLCPP_DEBUG(logger_, "Removing Object '%s' from CollisionEnvDistanceField", id.c_str()); dfce->posed_body_point_decompositions_.erase(id); } } diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 4df2fca98d..caedd76c9a 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -22,6 +22,7 @@ target_link_libraries(moveit_constraint_samplers moveit_kinematic_constraints moveit_kinematics_base moveit_planning_scene + moveit_utils ) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index 809e16cf23..69b022aacb 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -37,10 +37,18 @@ #include #include #include +#include namespace constraint_samplers { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.constraint_sampler"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("constraint_sampler"); + return logger; +} +} // namespace constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name) @@ -48,7 +56,7 @@ constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene:: { if (!jmg_) { - RCLCPP_ERROR(LOGGER, "A JointModelGroup should have been specified for the constraint sampler"); + RCLCPP_ERROR(getLogger(), "A JointModelGroup should have been specified for the constraint sampler"); } } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 400f0ddb9e..c187505d3b 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -40,10 +40,18 @@ #include #include #include +#include namespace constraint_samplers { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.constraint_sampler_manager"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("constraint_sampler_manager"); + return logger; +} +} // namespace ConstraintSamplerPtr ConstraintSamplerManager::selectSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, @@ -68,14 +76,15 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni return ConstraintSamplerPtr(); std::stringstream ss; ss << constr.name; - RCLCPP_DEBUG(LOGGER, "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", + RCLCPP_DEBUG(getLogger(), + "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", jmg->getName().c_str(), ss.str().c_str()); ConstraintSamplerPtr joint_sampler; // location to put chosen joint sampler if needed // if there are joint constraints, we could possibly get a sampler from those if (!constr.joint_constraints.empty()) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "There are joint constraints specified. " "Attempting to construct a JointConstraintSampler for group '%s'", jmg->getName().c_str()); @@ -116,7 +125,8 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni JointConstraintSamplerPtr sampler = std::make_shared(scene, jmg->getName()); if (sampler->configure(jc)) { - RCLCPP_DEBUG(LOGGER, "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str()); + RCLCPP_DEBUG(getLogger(), "Allocated a sampler satisfying joint constraints for group '%s'", + jmg->getName().c_str()); return sampler; } } @@ -128,7 +138,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni JointConstraintSamplerPtr sampler = std::make_shared(scene, jmg->getName()); if (sampler->configure(jc)) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "Temporary sampler satisfying joint constraints for group '%s' allocated. " "Looking for different types of constraints before returning though.", jmg->getName().c_str()); @@ -149,7 +159,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni // should be used if (ik_alloc) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "There is an IK allocator for '%s'. " "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); @@ -189,7 +199,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni { // assign the link to a new constraint sampler used_l[constr.position_constraints[p].link_name] = iks; - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "Allocated an IK-based sampler for group '%s' " "satisfying position and orientation constraints on link '%s'", jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str()); @@ -226,7 +236,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[position_constraint.link_name] = iks; - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "Allocated an IK-based sampler for group '%s' " "satisfying position constraints on link '%s'", jmg->getName().c_str(), position_constraint.link_name.c_str()); @@ -258,7 +268,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[orientation_constraint.link_name] = iks; - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "Allocated an IK-based sampler for group '%s' " "satisfying orientation constraints on link '%s'", jmg->getName().c_str(), orientation_constraint.link_name.c_str()); @@ -281,7 +291,8 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni } else if (used_l.size() > 1) { - RCLCPP_DEBUG(LOGGER, "Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume", + RCLCPP_DEBUG(getLogger(), + "Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume", jmg->getName().c_str()); // find the sampler with the smallest sampling volume; delete the rest IKConstraintSamplerPtr iks = used_l.begin()->second; @@ -311,7 +322,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni // we now check to see if we can use samplers from subgroups if (!ik_subgroup_alloc.empty()) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "There are IK allocators for subgroups of group '%s'. " "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); @@ -351,12 +362,12 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni // if some matching constraints were found, construct the allocator if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty()) { - RCLCPP_DEBUG(LOGGER, "Attempting to construct a sampler for the '%s' subgroup of '%s'", + RCLCPP_DEBUG(getLogger(), "Attempting to construct a sampler for the '%s' subgroup of '%s'", it->first->getName().c_str(), jmg->getName().c_str()); ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr); if (cs) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "Constructed a sampler for the joints corresponding to group '%s', " "but part of group '%s'", it->first->getName().c_str(), jmg->getName().c_str()); @@ -367,8 +378,8 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni } if (some_sampler_valid) { - RCLCPP_DEBUG(LOGGER, "Constructing sampler for group '%s' as a union of %zu samplers", jmg->getName().c_str(), - samplers.size()); + RCLCPP_DEBUG(getLogger(), "Constructing sampler for group '%s' as a union of %zu samplers", + jmg->getName().c_str(), samplers.size()); return std::make_shared(scene, jmg->getName(), samplers); } } @@ -376,11 +387,11 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni // if we've gotten here, just return joint sampler if (joint_sampler) { - RCLCPP_DEBUG(LOGGER, "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str()); + RCLCPP_DEBUG(getLogger(), "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str()); return joint_sampler; } - RCLCPP_DEBUG(LOGGER, "No constraints sampler allocated for group '%s'", jmg->getName().c_str()); + RCLCPP_DEBUG(getLogger(), "No constraints sampler allocated for group '%s'", jmg->getName().c_str()); return ConstraintSamplerPtr(); } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index d173620483..0f3d4600ee 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -41,10 +41,18 @@ #include #include #include +#include namespace constraint_samplers { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.constraint_sampler_tools"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("constraint_sampler_tools"); + return logger; +} +} // namespace void visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, @@ -66,7 +74,7 @@ double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const moveit:: { if (!sampler) { - RCLCPP_ERROR(LOGGER, "No sampler specified for counting samples per second"); + RCLCPP_ERROR(getLogger(), "No sampler specified for counting samples per second"); return 0.0; } moveit::core::RobotState ks(reference_state); @@ -92,7 +100,7 @@ void visualizeDistribution(const ConstraintSamplerPtr& sampler, const moveit::co { if (!sampler) { - RCLCPP_ERROR(LOGGER, "No sampler specified for visualizing distribution of samples"); + RCLCPP_ERROR(getLogger(), "No sampler specified for visualizing distribution of samples"); return; } const moveit::core::LinkModel* lm = reference_state.getLinkModel(link_name); diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 27dfd6dc3e..38f83f2a8c 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -39,10 +39,18 @@ #include #include #include +#include namespace constraint_samplers { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.default_constraint_samplers"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("default_constraint_samplers"); + return logger; +} +} // namespace bool JointConstraintSampler::configure(const moveit_msgs::msg::Constraints& constr) { @@ -64,7 +72,7 @@ bool JointConstraintSampler::configure(const std::vector ji.max_bound_ + std::numeric_limits::epsilon()) { std::stringstream cs; joint_constraint.print(cs); - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "The constraints for joint '%s' are such that " "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n", jm->getName().c_str(), ji.min_bound_, ji.max_bound_); @@ -119,7 +127,7 @@ bool JointConstraintSampler::configure(const std::vectorenabled() && !sp.orientation_constraint_->enabled())) { - RCLCPP_WARN(LOGGER, "No enabled constraints in sampling pose"); + RCLCPP_WARN(getLogger(), "No enabled constraints in sampling pose"); return false; } @@ -264,8 +272,8 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) if (sampling_pose_.position_constraint_->getLinkModel()->getName() != sampling_pose_.orientation_constraint_->getLinkModel()->getName()) { - RCLCPP_ERROR(LOGGER, "Position and orientation constraints need to be specified for the same link " - "in order to use IK-based sampling"); + RCLCPP_ERROR(getLogger(), "Position and orientation constraints need to be specified for the same link " + "in order to use IK-based sampling"); return false; } } @@ -277,7 +285,7 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) kb_ = jmg_->getSolverInstance(); if (!kb_) { - RCLCPP_WARN(LOGGER, "No solver instance in setup"); + RCLCPP_WARN(getLogger(), "No solver instance in setup"); is_valid_ = false; return false; } @@ -356,7 +364,7 @@ bool IKConstraintSampler::loadIKSolver() { if (!kb_) { - RCLCPP_ERROR(LOGGER, "No IK solver"); + RCLCPP_ERROR(getLogger(), "No IK solver"); return false; } @@ -369,7 +377,7 @@ bool IKConstraintSampler::loadIKSolver() { if (!jmg_->getParentModel().hasLinkModel(ik_frame_)) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. " "Ignoring transformation (IK may fail)", ik_frame_.c_str()); @@ -421,7 +429,7 @@ bool IKConstraintSampler::loadIKSolver() if (wrong_link) { - RCLCPP_ERROR(LOGGER, "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.", + RCLCPP_ERROR(getLogger(), "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.", sampling_pose_.position_constraint_ ? sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() : sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(), @@ -437,8 +445,8 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q if (ks.dirtyLinkTransforms()) { // samplePose below requires accurate transforms - RCLCPP_ERROR(LOGGER, "IKConstraintSampler received dirty robot state, but valid transforms are required. " - "Failing."); + RCLCPP_ERROR(getLogger(), "IKConstraintSampler received dirty robot state, but valid transforms are required. " + "Failing."); return false; } @@ -459,14 +467,14 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q } if (!found) { - RCLCPP_ERROR(LOGGER, "Unable to sample a point inside the constraint region"); + RCLCPP_ERROR(getLogger(), "Unable to sample a point inside the constraint region"); return false; } } else { - RCLCPP_ERROR(LOGGER, "Unable to sample a point inside the constraint region. " - "Constraint region is empty when it should not be."); + RCLCPP_ERROR(getLogger(), "Unable to sample a point inside the constraint region. " + "Constraint region is empty when it should not be."); return false; } @@ -513,7 +521,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q else { /* The parameterization type should be validated in configure, so this should never happen. */ - RCLCPP_ERROR(LOGGER, "The parameterization type for the orientation constraints is invalid."); + RCLCPP_ERROR(getLogger(), "The parameterization type for the orientation constraints is invalid."); } // diff is isometry by construction // getDesiredRotationMatrix() returns a valid rotation matrix by contract @@ -582,7 +590,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo { if (!is_valid_) { - RCLCPP_WARN(LOGGER, "IKConstraintSampler not configured, won't sample"); + RCLCPP_WARN(getLogger(), "IKConstraintSampler not configured, won't sample"); return false; } @@ -604,7 +612,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo if (!samplePose(point, quat, reference_state, max_attempts)) { if (verbose_) - RCLCPP_INFO(LOGGER, "IK constraint sampler was unable to produce a pose to run IK for"); + RCLCPP_INFO(getLogger(), "IK constraint sampler was unable to produce a pose to run IK for"); return false; } @@ -696,11 +704,11 @@ bool IKConstraintSampler::callIK(const geometry_msgs::msg::Pose& ik_query, error.val != moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE && error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT) { - RCLCPP_ERROR(LOGGER, "IK solver failed with error %d", error.val); + RCLCPP_ERROR(getLogger(), "IK solver failed with error %d", error.val); } else if (verbose_) { - RCLCPP_INFO(LOGGER, "IK failed"); + RCLCPP_INFO(getLogger(), "IK failed"); } } return false; diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 9708afa24e..1a98aa9fd3 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -39,10 +39,18 @@ #include #include #include +#include namespace constraint_samplers { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.union_constraint_sampler"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("union_constraint_sampler"); + return logger; +} +} // namespace struct OrderSamplers { @@ -92,7 +100,7 @@ struct OrderSamplers } if (b_depends_on_a && a_depends_on_b) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Circular frame dependency! " "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')", a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str()); @@ -130,7 +138,7 @@ UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSce for (const std::string& fd : fds) frame_depends_.push_back(fd); - RCLCPP_DEBUG(LOGGER, "Union sampler for group '%s' includes sampler for group '%s'", jmg_->getName().c_str(), + RCLCPP_DEBUG(getLogger(), "Union sampler for group '%s' includes sampler for group '%s'", jmg_->getName().c_str(), sampler->getJointModelGroup()->getName().c_str()); } } diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index 5f32a8948a..27a12fe87a 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -7,7 +7,7 @@ target_include_directories(moveit_distance_field PUBLIC $ $ ) -target_link_libraries(moveit_distance_field moveit_macros) +target_link_libraries(moveit_distance_field moveit_macros moveit_utils) set_target_properties(moveit_distance_field PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_distance_field Boost diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index b455f5d002..758d439097 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -43,11 +43,18 @@ #include #include #include +#include namespace distance_field { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_distance_field.distance_field"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("distance_field"); + return logger; +} +} // namespace DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z) @@ -207,7 +214,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(getLogger(), "Problem dynamic casting shape that claims to be OcTree"); return false; } getOcTreePoints(oc->octree.get(), points); @@ -289,7 +296,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(getLogger(), "Move shape not supported for Octree"); return; } bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index a03478911a..575f01a88f 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -41,11 +41,18 @@ #include #include #include +#include namespace distance_field { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_distance_field.propagation_distance_field"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("propagation_distance_field"); + return logger; +} +} // namespace PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, @@ -99,26 +106,26 @@ void PropagationDistanceField::initialize() void PropagationDistanceField::print(const VoxelSet& set) { - RCLCPP_DEBUG(LOGGER, "["); + RCLCPP_DEBUG(getLogger(), "["); 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(getLogger(), "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z()); } - RCLCPP_DEBUG(LOGGER, "] size=%u\n", static_cast(set.size())); + RCLCPP_DEBUG(getLogger(), "] size=%u\n", static_cast(set.size())); } void PropagationDistanceField::print(const EigenSTL::vector_Vector3d& points) { - RCLCPP_DEBUG(LOGGER, "["); + RCLCPP_DEBUG(getLogger(), "["); 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(getLogger(), "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z()); } - RCLCPP_DEBUG(LOGGER, "] size=%u\n", static_cast(points.size())); + RCLCPP_DEBUG(getLogger(), "] size=%u\n", static_cast(points.size())); } void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector3d& old_points, @@ -402,7 +409,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", vptr->update_direction_); + RCLCPP_ERROR(getLogger(), "PROGRAMMING ERROR: Invalid update direction detected: %d", vptr->update_direction_); continue; } @@ -459,7 +466,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", vptr->update_direction_); + RCLCPP_ERROR(getLogger(), "PROGRAMMING ERROR: Invalid update direction detected: %d", vptr->update_direction_); continue; } diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt index 334b021690..7e83f476a5 100644 --- a/moveit_core/dynamics_solver/CMakeLists.txt +++ b/moveit_core/dynamics_solver/CMakeLists.txt @@ -14,6 +14,7 @@ ament_target_dependencies(moveit_dynamics_solver ) target_link_libraries(moveit_dynamics_solver moveit_robot_state + moveit_utils ) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 677952c201..0a5cb0a861 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -41,10 +41,18 @@ #include #include #include +#include namespace dynamics_solver { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_dynamics_solver.dynamics_solver"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("dynamics_solver"); + return logger; +} +} // namespace namespace { @@ -75,14 +83,14 @@ DynamicsSolver::DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_mod if (!joint_model_group_->isChain()) { - RCLCPP_ERROR(LOGGER, "Group '%s' is not a chain. Will not initialize dynamics solver", group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Group '%s' is not a chain. Will not initialize dynamics solver", group_name.c_str()); joint_model_group_ = nullptr; return; } if (!joint_model_group_->getMimicJointModels().empty()) { - RCLCPP_ERROR(LOGGER, "Group '%s' has a mimic joint. Will not initialize dynamics solver", group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Group '%s' has a mimic joint. Will not initialize dynamics solver", group_name.c_str()); joint_model_group_ = nullptr; return; } @@ -90,7 +98,7 @@ DynamicsSolver::DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_mod const moveit::core::JointModel* joint = joint_model_group_->getJointRoots()[0]; if (!joint->getParentLinkModel()) { - RCLCPP_ERROR(LOGGER, "Group '%s' does not have a parent link", group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Group '%s' does not have a parent link", group_name.c_str()); joint_model_group_ = nullptr; return; } @@ -98,7 +106,7 @@ DynamicsSolver::DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_mod base_name_ = joint->getParentLinkModel()->getName(); tip_name_ = joint_model_group_->getLinkModelNames().back(); - RCLCPP_DEBUG(LOGGER, "Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str()); + RCLCPP_DEBUG(getLogger(), "Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str()); const urdf::ModelInterfaceSharedPtr urdf_model = robot_model_->getURDF(); const srdf::ModelConstSharedPtr srdf_model = robot_model_->getSRDF(); @@ -106,13 +114,13 @@ DynamicsSolver::DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_mod if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree)) { - RCLCPP_ERROR(LOGGER, "Could not initialize tree object"); + RCLCPP_ERROR(getLogger(), "Could not initialize tree object"); joint_model_group_ = nullptr; return; } if (!tree.getChain(base_name_, tip_name_, kdl_chain_)) { - RCLCPP_ERROR(LOGGER, "Could not initialize chain object"); + RCLCPP_ERROR(getLogger(), "Could not initialize chain object"); joint_model_group_ = nullptr; return; } @@ -139,7 +147,7 @@ DynamicsSolver::DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_mod KDL::Vector gravity(gravity_vector.x, gravity_vector.y, gravity_vector.z); // \todo Not sure if KDL expects the negative of this (Sachin) gravity_ = gravity.Norm(); - RCLCPP_DEBUG(LOGGER, "Gravity norm set to %f", gravity_); + RCLCPP_DEBUG(getLogger(), "Gravity norm set to %f", gravity_); chain_id_solver_ = std::make_shared(kdl_chain_, gravity); } @@ -151,33 +159,33 @@ bool DynamicsSolver::getTorques(const std::vector& joint_angles, const s { if (!joint_model_group_) { - RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. " - "Check error logs."); + RCLCPP_DEBUG(getLogger(), "Did not construct DynamicsSolver object properly. " + "Check error logs."); return false; } if (joint_angles.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_); + RCLCPP_ERROR(getLogger(), "Joint angles vector should be size %d", num_joints_); return false; } if (joint_velocities.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Joint velocities vector should be size %d", num_joints_); + RCLCPP_ERROR(getLogger(), "Joint velocities vector should be size %d", num_joints_); return false; } if (joint_accelerations.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Joint accelerations vector should be size %d", num_joints_); + RCLCPP_ERROR(getLogger(), "Joint accelerations vector should be size %d", num_joints_); return false; } if (wrenches.size() != num_segments_) { - RCLCPP_ERROR(LOGGER, "Wrenches vector should be size %d", num_segments_); + RCLCPP_ERROR(getLogger(), "Wrenches vector should be size %d", num_segments_); return false; } if (torques.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Torques vector should be size %d", num_joints_); + RCLCPP_ERROR(getLogger(), "Torques vector should be size %d", num_joints_); return false; } @@ -205,7 +213,7 @@ bool DynamicsSolver::getTorques(const std::vector& joint_angles, const s if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0) { - RCLCPP_ERROR(LOGGER, "Something went wrong computing torques"); + RCLCPP_ERROR(getLogger(), "Something went wrong computing torques"); return false; } @@ -220,13 +228,13 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub { if (!joint_model_group_) { - RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. " - "Check error logs."); + RCLCPP_DEBUG(getLogger(), "Did not construct DynamicsSolver object properly. " + "Check error logs."); return false; } if (joint_angles.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_); + RCLCPP_ERROR(getLogger(), "Joint angles vector should be size %d", num_joints_); return false; } std::vector joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0); @@ -254,7 +262,7 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); - RCLCPP_DEBUG(LOGGER, "New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, + RCLCPP_DEBUG(getLogger(), "New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z); if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques)) @@ -266,9 +274,9 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub double payload_joint = std::max((max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i]), (-max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i])); // because we set the payload to 1.0 - RCLCPP_DEBUG(LOGGER, "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i], max_torques_[i], - zero_torques[i]); - RCLCPP_DEBUG(LOGGER, "Joint: %d, Payload Allowed (N): %f", i, payload_joint); + RCLCPP_DEBUG(getLogger(), "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i], + max_torques_[i], zero_torques[i]); + RCLCPP_DEBUG(getLogger(), "Joint: %d, Payload Allowed (N): %f", i, payload_joint); if (payload_joint < min_payload) { min_payload = payload_joint; @@ -276,7 +284,7 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub } } payload = min_payload / gravity_; - RCLCPP_DEBUG(LOGGER, "Max payload (kg): %f", payload); + RCLCPP_DEBUG(getLogger(), "Max payload (kg): %f", payload); return true; } @@ -285,18 +293,18 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, { if (!joint_model_group_) { - RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. " - "Check error logs."); + RCLCPP_DEBUG(getLogger(), "Did not construct DynamicsSolver object properly. " + "Check error logs."); return false; } if (joint_angles.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_); + RCLCPP_ERROR(getLogger(), "Joint angles vector should be size %d", num_joints_); return false; } if (joint_torques.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Joint torques vector should be size %d", num_joints_); + RCLCPP_ERROR(getLogger(), "Joint torques vector should be size %d", num_joints_); return false; } std::vector joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0); @@ -310,7 +318,7 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); - RCLCPP_DEBUG(LOGGER, "New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, + RCLCPP_DEBUG(getLogger(), "New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z); return getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques); diff --git a/moveit_core/exceptions/CMakeLists.txt b/moveit_core/exceptions/CMakeLists.txt index 5283200839..aca217c569 100644 --- a/moveit_core/exceptions/CMakeLists.txt +++ b/moveit_core/exceptions/CMakeLists.txt @@ -10,5 +10,6 @@ ament_target_dependencies(moveit_exceptions urdfdom urdfdom_headers ) +target_link_libraries(moveit_exceptions moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/exceptions/src/exceptions.cpp b/moveit_core/exceptions/src/exceptions.cpp index b0f748a830..33ef0f8508 100644 --- a/moveit_core/exceptions/src/exceptions.cpp +++ b/moveit_core/exceptions/src/exceptions.cpp @@ -37,19 +37,19 @@ #include #include #include +#include // Logger namespace moveit { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_exceptions.exceptions"); ConstructException::ConstructException(const std::string& what_arg) : std::runtime_error(what_arg) { - RCLCPP_ERROR(LOGGER, "Error during construction of object: %s\nException thrown.", what_arg.c_str()); + RCLCPP_ERROR(getLogger(), "Error during construction of object: %s\nException thrown.", what_arg.c_str()); } Exception::Exception(const std::string& what_arg) : std::runtime_error(what_arg) { - RCLCPP_ERROR(LOGGER, "%s\nException thrown.", what_arg.c_str()); + RCLCPP_ERROR(getLogger(), "%s\nException thrown.", what_arg.c_str()); } } // namespace moveit diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 68ac2be507..58d5194ec6 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -49,14 +49,21 @@ #include #include #include +#include #include #include namespace kinematic_constraints { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kinematic_constraints.kinematic_constraints"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("kinematic_constraints"); + return logger; +} +} // namespace static double normalizeAngle(double angle) { @@ -139,7 +146,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) // testing tolerances first if (jc.tolerance_above < 0.0 || jc.tolerance_below < 0.0) { - RCLCPP_WARN(LOGGER, "JointConstraint tolerance values must be positive."); + RCLCPP_WARN(getLogger(), "JointConstraint tolerance values must be positive."); joint_model_ = nullptr; return false; } @@ -170,12 +177,12 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) // check if the joint has 1 DOF (the only kind we can handle) if (joint_model_->getVariableCount() == 0) { - RCLCPP_ERROR(LOGGER, "Joint '%s' has no parameters to constrain", jc.joint_name.c_str()); + RCLCPP_ERROR(getLogger(), "Joint '%s' has no parameters to constrain", jc.joint_name.c_str()); joint_model_ = nullptr; } else if (joint_model_->getVariableCount() > 1) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Joint '%s' has more than one parameter to constrain. " "This type of constraint is not supported.", jc.joint_name.c_str()); @@ -196,7 +203,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) } if (found < 0) { - RCLCPP_ERROR(LOGGER, "Local variable name '%s' is not known to joint '%s'", local_variable_name_.c_str(), + RCLCPP_ERROR(getLogger(), "Local variable name '%s' is not known to joint '%s'", local_variable_name_.c_str(), joint_model_->getName().c_str()); joint_model_ = nullptr; } @@ -238,7 +245,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) { joint_position_ = bounds.min_position_; joint_tolerance_above_ = std::numeric_limits::epsilon(); - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Joint %s is constrained to be below the minimum bounds. " "Assuming minimum bounds instead.", jc.joint_name.c_str()); @@ -247,7 +254,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) { joint_position_ = bounds.max_position_; joint_tolerance_below_ = std::numeric_limits::epsilon(); - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Joint %s is constrained to be above the maximum bounds. " "Assuming maximum bounds instead.", jc.joint_name.c_str()); @@ -256,7 +263,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) if (jc.weight <= std::numeric_limits::epsilon()) { - RCLCPP_WARN(LOGGER, "The weight on constraint for joint '%s' is very near zero. Setting to 1.0.", + RCLCPP_WARN(getLogger(), "The weight on constraint for joint '%s' is very near zero. Setting to 1.0.", jc.joint_name.c_str()); constraint_weight_ = 1.0; } @@ -310,7 +317,7 @@ ConstraintEvaluationResult JointConstraint::decide(const moveit::core::RobotStat dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits::epsilon()); if (verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, " "tolerance_above: %f, tolerance_below: %f", result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position, @@ -359,14 +366,14 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p link_model_ = robot_model_->getLinkModel(pc.link_name); if (link_model_ == nullptr) { - RCLCPP_WARN(LOGGER, "Position constraint link model %s not found in kinematic model. Constraint invalid.", + RCLCPP_WARN(getLogger(), "Position constraint link model %s not found in kinematic model. Constraint invalid.", pc.link_name.c_str()); return false; } if (pc.header.frame_id.empty()) { - RCLCPP_WARN(LOGGER, "No frame specified for position constraint on link '%s'!", pc.link_name.c_str()); + RCLCPP_WARN(getLogger(), "No frame specified for position constraint on link '%s'!", pc.link_name.c_str()); return false; } @@ -392,7 +399,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p { if (pc.constraint_region.primitive_poses.size() <= i) { - RCLCPP_WARN(LOGGER, "Constraint region message does not contain enough primitive poses"); + RCLCPP_WARN(getLogger(), "Constraint region message does not contain enough primitive poses"); continue; } Eigen::Isometry3d t; @@ -409,7 +416,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p constraint_region_.push_back(body); } else - RCLCPP_WARN(LOGGER, "Could not construct primitive shape %zu", i); + RCLCPP_WARN(getLogger(), "Could not construct primitive shape %zu", i); } // load meshes @@ -420,7 +427,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p { if (pc.constraint_region.mesh_poses.size() <= i) { - RCLCPP_WARN(LOGGER, "Constraint region message does not contain enough primitive poses"); + RCLCPP_WARN(getLogger(), "Constraint region message does not contain enough primitive poses"); continue; } Eigen::Isometry3d t; @@ -437,13 +444,13 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p } else { - RCLCPP_WARN(LOGGER, "Could not construct mesh shape %zu", i); + RCLCPP_WARN(getLogger(), "Could not construct mesh shape %zu", i); } } if (pc.weight <= std::numeric_limits::epsilon()) { - RCLCPP_WARN(LOGGER, "The weight on position constraint for link '%s' is near zero. Setting to 1.0.", + RCLCPP_WARN(getLogger(), "The weight on position constraint for link '%s' is near zero. Setting to 1.0.", pc.link_name.c_str()); constraint_weight_ = 1.0; } @@ -505,10 +512,10 @@ static inline ConstraintEvaluationResult finishPositionConstraintDecision(const double dz = desired.z() - pt.z(); if (verbose) { - RCLCPP_INFO(LOGGER, "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", + RCLCPP_INFO(getLogger(), "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(), pt.z()); - RCLCPP_INFO(LOGGER, "Differences %g %g %g", dx, dy, dz); + RCLCPP_INFO(getLogger(), "Differences %g %g %g", dx, dy, dz); } return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz)); } @@ -594,14 +601,14 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra link_model_ = robot_model_->getLinkModel(oc.link_name); if (!link_model_) { - RCLCPP_WARN(LOGGER, "Could not find link model for link name %s", oc.link_name.c_str()); + RCLCPP_WARN(getLogger(), "Could not find link model for link name %s", oc.link_name.c_str()); return false; } Eigen::Quaterniond q; tf2::fromMsg(oc.orientation, q); if (fabs(q.norm() - 1.0) > 1e-3) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, " "%f. Assuming identity instead.", oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w); @@ -609,7 +616,7 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra } if (oc.header.frame_id.empty()) - RCLCPP_WARN(LOGGER, "No frame specified for position constraint on link '%s'!", oc.link_name.c_str()); + RCLCPP_WARN(getLogger(), "No frame specified for position constraint on link '%s'!", oc.link_name.c_str()); if (tf.isFixedFrame(oc.header.frame_id)) { @@ -627,12 +634,12 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra } std::stringstream matrix_str; matrix_str << desired_rotation_matrix_; - RCLCPP_DEBUG(LOGGER, "The desired rotation matrix for link '%s' in frame %s is:\n%s", oc.link_name.c_str(), + RCLCPP_DEBUG(getLogger(), "The desired rotation matrix for link '%s' in frame %s is:\n%s", oc.link_name.c_str(), desired_rotation_frame_id_.c_str(), matrix_str.str().c_str()); if (oc.weight <= std::numeric_limits::epsilon()) { - RCLCPP_WARN(LOGGER, "The weight on orientation constraint for link '%s' is near zero. Setting to 1.0.", + RCLCPP_WARN(getLogger(), "The weight on orientation constraint for link '%s' is near zero. Setting to 1.0.", oc.link_name.c_str()); constraint_weight_ = 1.0; } @@ -646,20 +653,20 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra if (parameterization_type_ != moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES && parameterization_type_ != moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Unknown parameterization for orientation constraint tolerance, using default (XYZ_EULER_ANGLES)."); parameterization_type_ = moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES; } absolute_x_axis_tolerance_ = fabs(oc.absolute_x_axis_tolerance); if (absolute_x_axis_tolerance_ < std::numeric_limits::epsilon()) - RCLCPP_WARN(LOGGER, "Near-zero value for absolute_x_axis_tolerance"); + RCLCPP_WARN(getLogger(), "Near-zero value for absolute_x_axis_tolerance"); absolute_y_axis_tolerance_ = fabs(oc.absolute_y_axis_tolerance); if (absolute_y_axis_tolerance_ < std::numeric_limits::epsilon()) - RCLCPP_WARN(LOGGER, "Near-zero value for absolute_y_axis_tolerance"); + RCLCPP_WARN(getLogger(), "Near-zero value for absolute_y_axis_tolerance"); absolute_z_axis_tolerance_ = fabs(oc.absolute_z_axis_tolerance); if (absolute_z_axis_tolerance_ < std::numeric_limits::epsilon()) - RCLCPP_WARN(LOGGER, "Near-zero value for absolute_z_axis_tolerance"); + RCLCPP_WARN(getLogger(), "Near-zero value for absolute_z_axis_tolerance"); return link_model_ != nullptr; } @@ -751,7 +758,7 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob else { /* The parameterization type should be validated in configure, so this should never happen. */ - RCLCPP_ERROR(LOGGER, "The parameterization type for the orientation constraints is invalid."); + RCLCPP_ERROR(getLogger(), "The parameterization type for the orientation constraints is invalid."); } bool result = xyz_rotation(2) < absolute_z_axis_tolerance_ + std::numeric_limits::epsilon() && @@ -762,7 +769,7 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob { Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).linear()); Eigen::Quaterniond q_des(desired_rotation_matrix_); - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion " "actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f", result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(), @@ -812,11 +819,11 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain target_radius_ = fabs(vc.target_radius); if (vc.target_radius <= std::numeric_limits::epsilon()) - RCLCPP_WARN(LOGGER, "The radius of the target disc that must be visible should be strictly positive"); + RCLCPP_WARN(getLogger(), "The radius of the target disc that must be visible should be strictly positive"); if (vc.cone_sides < 3) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "The number of sides for the visibility region must be 3 or more. " "Assuming 3 sides instead of the specified %d", vc.cone_sides); @@ -864,7 +871,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain if (vc.weight <= std::numeric_limits::epsilon()) { - RCLCPP_WARN(LOGGER, "The weight of visibility constraint is near zero. Setting to 1.0."); + RCLCPP_WARN(getLogger(), "The weight of visibility constraint is near zero. Setting to 1.0."); constraint_weight_ = 1.0; } else @@ -1081,8 +1088,8 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo { if (verbose) { - RCLCPP_INFO(LOGGER, "Visibility constraint is violated because the sensor is looking at " - "the wrong side"); + RCLCPP_INFO(getLogger(), "Visibility constraint is violated because the sensor is looking at " + "the wrong side"); } return ConstraintEvaluationResult(false, 0.0); } @@ -1090,7 +1097,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo { if (verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Visibility constraint is violated because the view angle is %lf " "(above the maximum allowed of %lf)", ang, max_view_angle_); @@ -1109,8 +1116,8 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo { if (verbose) { - RCLCPP_INFO(LOGGER, "Visibility constraint is violated because the sensor is looking at " - "the wrong side"); + RCLCPP_INFO(getLogger(), "Visibility constraint is violated because the sensor is looking at " + "the wrong side"); } return ConstraintEvaluationResult(false, 0.0); } @@ -1120,7 +1127,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo { if (verbose) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Visibility constraint is violated because the range angle is %lf " "(above the maximum allowed of %lf)", ang, max_range_angle_); @@ -1135,7 +1142,8 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo shapes::Mesh* m = getVisibilityCone(tform_world_to_sensor, tform_world_to_target); if (!m) { - RCLCPP_ERROR(LOGGER, "Visibility constraint is violated because we could not create the visibility cone mesh."); + RCLCPP_ERROR(getLogger(), + "Visibility constraint is violated because we could not create the visibility cone mesh."); return ConstraintEvaluationResult(false, 0.0); } @@ -1162,7 +1170,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo { std::stringstream ss; m->print(ss); - RCLCPP_INFO(LOGGER, "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", + RCLCPP_INFO(getLogger(), "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", res.collision ? "not " : "", ss.str().c_str()); } @@ -1185,7 +1193,7 @@ bool VisibilityConstraint::decideContact(const collision_detection::Contact& con (moveit::core::Transforms::sameFrame(contact.body_name_1, sensor_frame_id_) || moveit::core::Transforms::sameFrame(contact.body_name_1, target_frame_id_))) { - RCLCPP_DEBUG(LOGGER, "Accepted collision with either sensor or target"); + RCLCPP_DEBUG(getLogger(), "Accepted collision with either sensor or target"); return true; } if (contact.body_type_2 == collision_detection::BodyTypes::ROBOT_LINK && @@ -1193,7 +1201,7 @@ bool VisibilityConstraint::decideContact(const collision_detection::Contact& con (moveit::core::Transforms::sameFrame(contact.body_name_2, sensor_frame_id_) || moveit::core::Transforms::sameFrame(contact.body_name_2, target_frame_id_))) { - RCLCPP_DEBUG(LOGGER, "Accepted collision with either sensor or target"); + RCLCPP_DEBUG(getLogger(), "Accepted collision with either sensor or target"); return true; } return false; diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index d2db990b96..18e4f14684 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -52,8 +53,14 @@ using namespace moveit::core; namespace kinematic_constraints { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kinematic_constraints.utils"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("moveit_kinematic_constraints"); + return logger; +} +} // namespace moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints& first, const moveit_msgs::msg::Constraints& second) @@ -78,7 +85,8 @@ moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constrain double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above); if (low > high) { - RCLCPP_ERROR(LOGGER, "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.", + RCLCPP_ERROR(getLogger(), + "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.", a.joint_name.c_str()); } else @@ -290,7 +298,7 @@ bool updateOrientationConstraint(moveit_msgs::msg::Constraints& constraints, con { if (quat.header.frame_id.empty()) { - RCLCPP_ERROR(LOGGER, "Cannot update orientation constraint, frame_id in the header is empty"); + RCLCPP_ERROR(getLogger(), "Cannot update orientation constraint, frame_id in the header is empty"); return false; } constraint.header = quat.header; @@ -321,7 +329,7 @@ bool updatePositionConstraint(moveit_msgs::msg::Constraints& constraints, const { if (goal_point.header.frame_id.empty()) { - RCLCPP_ERROR(LOGGER, "Cannot update position constraint, frame_id in the header is empty"); + RCLCPP_ERROR(getLogger(), "Cannot update position constraint, frame_id in the header is empty"); return false; } constraint.header = goal_point.header; @@ -557,7 +565,7 @@ static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::v const auto constraint_param = "constraints." + constraint_id; if (!node->has_parameter(constraint_param + ".type")) { - RCLCPP_ERROR(LOGGER, "constraint parameter \"%s\" does not specify its type", constraint_param.c_str()); + RCLCPP_ERROR(getLogger(), "constraint parameter \"%s\" does not specify its type", constraint_param.c_str()); return false; } std::string constraint_type; @@ -588,7 +596,7 @@ static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::v } else { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to process unknown constraint type: " << constraint_type); + RCLCPP_ERROR_STREAM(getLogger(), "Unable to process unknown constraint type: " << constraint_type); return false; } } diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index 114e51eb35..13dc3c9c9c 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -3,7 +3,7 @@ target_include_directories(moveit_kinematics_base PUBLIC $ $ ) -target_link_libraries(moveit_kinematics_base moveit_macros moveit_robot_model) +target_link_libraries(moveit_kinematics_base moveit_macros moveit_robot_model moveit_utils) include(GenerateExportHeader) generate_export_header(moveit_kinematics_base) target_include_directories(moveit_kinematics_base PUBLIC $) # for this library diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 691ba11983..b5cc2fd7df 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -43,6 +43,7 @@ #include #include #include +#include #include @@ -146,7 +147,6 @@ MOVEIT_CLASS_FORWARD(KinematicsBase); // Defines KinematicsBasePtr, ConstPtr, W class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase { public: - static const rclcpp::Logger LOGGER; static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */ static const double DEFAULT_TIMEOUT; /* = 1.0 */ @@ -322,7 +322,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase } // Otherwise throw error because this function should have been implemented - RCLCPP_ERROR(LOGGER, "This kinematic solver does not support searchPositionIK with multiple poses"); + RCLCPP_ERROR(moveit::getLogger(), "This kinematic solver does not support searchPositionIK with multiple poses"); return false; } @@ -362,7 +362,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options, context_state); } - RCLCPP_ERROR(LOGGER, "This kinematic solver does not support IK solution cost functions"); + RCLCPP_ERROR(moveit::getLogger(), "This kinematic solver does not support IK solution cost functions"); return false; } @@ -435,8 +435,8 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase { if (tip_frames_.size() > 1) { - RCLCPP_ERROR(LOGGER, "This kinematic solver has more than one tip frame, " - "do not call getTipFrame()"); + RCLCPP_ERROR(moveit::getLogger(), "This kinematic solver has more than one tip frame, " + "do not call getTipFrame()"); } return tip_frames_[0]; diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index 75811a382c..a5c1503325 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -37,11 +37,19 @@ #include #include #include +#include namespace kinematics { -// Logger -const rclcpp::Logger KinematicsBase::LOGGER = rclcpp::get_logger("moveit_kinematics_base.kinematics_base"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("kinematics_base"); + return logger; +} +} // namespace + const double KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION = 0.1; const double KinematicsBase::DEFAULT_TIMEOUT = 1.0; @@ -86,7 +94,7 @@ bool KinematicsBase::initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*base_frame*/, const std::vector& /*tip_frames*/, double /*search_discretization*/) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "IK plugin for group '%s' relies on deprecated API. " "Please implement initialize(rclcpp::Node::SharedPtr, RobotModel, ...).", group_name.c_str()); @@ -171,14 +179,14 @@ bool KinematicsBase::getPositionIK(const std::vector& if (ik_poses.size() != 1) { - RCLCPP_ERROR(LOGGER, "This kinematic solver does not support getPositionIK for multiple tips"); + RCLCPP_ERROR(getLogger(), "This kinematic solver does not support getPositionIK for multiple tips"); result.kinematic_error = KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED; return false; } if (ik_poses.empty()) { - RCLCPP_ERROR(LOGGER, "Input ik_poses array is empty"); + RCLCPP_ERROR(getLogger(), "Input ik_poses array is empty"); result.kinematic_error = KinematicErrors::EMPTY_TIP_POSES; return false; } diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index dee91e75d1..52fbbd8e29 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -13,6 +13,7 @@ ament_target_dependencies(moveit_kinematics_metrics target_link_libraries(moveit_kinematics_metrics moveit_robot_model moveit_robot_state + moveit_utils ) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index d6d89c07f7..188a6e067b 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -41,10 +41,18 @@ #include #include #include +#include namespace kinematics_metrics { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kinematics_metrics.kinematics_metrics"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("kinematics_metrics"); + return logger; +} +} // namespace double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState& state, const moveit::core::JointModelGroup* joint_model_group) const @@ -131,7 +139,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const moveit::core::RobotState& s manipulability_index = 1.0; for (unsigned int i = 0; i < singular_values.rows(); ++i) { - RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(getLogger(), "Singular value: %d %f", i, singular_values(i, 0)); manipulability_index *= singular_values(i, 0); } // Get manipulability index @@ -154,7 +162,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const moveit::core::RobotState& s manipulability_index = 1.0; for (unsigned int i = 0; i < singular_values.rows(); ++i) { - RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(getLogger(), "Singular value: %d %f", i, singular_values(i, 0)); manipulability_index *= singular_values(i, 0); } // Get manipulability index @@ -236,7 +244,7 @@ bool KinematicsMetrics::getManipulability(const moveit::core::RobotState& state, Eigen::MatrixXd singular_values = svdsolver.singularValues(); for (int i = 0; i < singular_values.rows(); ++i) { - RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(getLogger(), "Singular value: %d %f", i, singular_values(i, 0)); } manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff(); @@ -248,7 +256,7 @@ bool KinematicsMetrics::getManipulability(const moveit::core::RobotState& state, Eigen::MatrixXd singular_values = svdsolver.singularValues(); for (int i = 0; i < singular_values.rows(); ++i) { - RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(getLogger(), "Singular value: %d %f", i, singular_values(i, 0)); } manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff(); } diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 5709f0b2aa..367f9cb6fc 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -39,10 +39,18 @@ #include #include #include +#include namespace planning_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planning_interface.planning_interface"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("planning_interface"); + return logger; +} +} // namespace namespace { @@ -84,14 +92,14 @@ void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request) request_ = request; if (request_.allowed_planning_time <= 0.0) { - RCLCPP_INFO(LOGGER, "The timeout for planning must be positive (%lf specified). Assuming one second instead.", + RCLCPP_INFO(getLogger(), "The timeout for planning must be positive (%lf specified). Assuming one second instead.", request_.allowed_planning_time); request_.allowed_planning_time = 1.0; } if (request_.num_planning_attempts < 0) { - RCLCPP_ERROR(LOGGER, "The number of desired planning attempts should be positive. " - "Assuming one attempt."); + RCLCPP_ERROR(getLogger(), "The number of desired planning attempts should be positive. " + "Assuming one attempt."); } request_.num_planning_attempts = std::max(1, request_.num_planning_attempts); } diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index c60986fbb6..27fde1df57 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -51,10 +51,18 @@ #include #include #include +#include namespace planning_scene { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planning_scene.planning_scene"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("planning_scene"); + return logger; +} +} // namespace const std::string PlanningScene::OCTOMAP_NS = ""; const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)"; @@ -80,12 +88,12 @@ bool readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) double x, y, z, rx, ry, rz, rw; if (!(in >> x >> y >> z)) { - RCLCPP_ERROR(LOGGER, "Improperly formatted translation in scene geometry file"); + RCLCPP_ERROR(getLogger(), "Improperly formatted translation in scene geometry file"); return false; } if (!(in >> rx >> ry >> rz >> rw)) { - RCLCPP_ERROR(LOGGER, "Improperly formatted rotation in scene geometry file"); + RCLCPP_ERROR(getLogger(), "Improperly formatted rotation in scene geometry file"); return false; } pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz); @@ -279,7 +287,7 @@ PlanningScene::getCollisionEnv(const std::string& collision_detector_name) const { if (collision_detector_name != getCollisionDetectorName()) { - RCLCPP_ERROR(LOGGER, "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead", + RCLCPP_ERROR(getLogger(), "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead", collision_detector_name.c_str(), collision_detector_->alloc_->getName().c_str()); return collision_detector_->getCollisionEnv(); } @@ -292,7 +300,7 @@ PlanningScene::getCollisionEnvUnpadded(const std::string& collision_detector_nam { if (collision_detector_name != getCollisionDetectorName()) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Could not get CollisionRobotUnpadded named '%s'. " "Returning active CollisionRobotUnpadded '%s' instead", collision_detector_name.c_str(), collision_detector_->alloc_->getName().c_str()); @@ -816,7 +824,7 @@ bool PlanningScene::getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) c octomap.origin = tf2::toMsg(map->shape_poses_[0]); return true; } - RCLCPP_ERROR(LOGGER, "Unexpected number of shapes in octomap collision object. Not including '%s' object", + RCLCPP_ERROR(getLogger(), "Unexpected number of shapes in octomap collision object. Not including '%s' object", OCTOMAP_NS.c_str()); } return false; @@ -981,7 +989,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet { if (!in.good() || in.eof()) { - RCLCPP_ERROR(LOGGER, "Bad input stream when loading scene geometry"); + RCLCPP_ERROR(getLogger(), "Bad input stream when loading scene geometry"); return false; } // Read scene name @@ -1008,7 +1016,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet in >> marker; if (!in.good() || in.eof()) { - RCLCPP_ERROR(LOGGER, "Bad input stream when loading marker in scene geometry"); + RCLCPP_ERROR(getLogger(), "Bad input stream when loading marker in scene geometry"); return false; } if (marker == "*") // Start of new object @@ -1017,7 +1025,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet std::getline(in, object_id); if (!in.good() || in.eof()) { - RCLCPP_ERROR(LOGGER, "Bad input stream when loading object_id in scene geometry"); + RCLCPP_ERROR(getLogger(), "Bad input stream when loading object_id in scene geometry"); return false; } boost::algorithm::trim(object_id); @@ -1026,7 +1034,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet pose.setIdentity(); if (uses_new_scene_format && !utilities::readPoseFromText(in, pose)) { - RCLCPP_ERROR(LOGGER, "Failed to read object pose from scene file"); + RCLCPP_ERROR(getLogger(), "Failed to read object pose from scene file"); return false; } pose = offset * pose; // Transform pose by input pose offset @@ -1040,18 +1048,18 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet const auto shape = shapes::ShapeConstPtr(shapes::constructShapeFromText(in)); if (!shape) { - RCLCPP_ERROR(LOGGER, "Failed to load shape from scene file"); + RCLCPP_ERROR(getLogger(), "Failed to load shape from scene file"); return false; } if (!utilities::readPoseFromText(in, pose)) { - RCLCPP_ERROR(LOGGER, "Failed to read pose from scene file"); + RCLCPP_ERROR(getLogger(), "Failed to read pose from scene file"); return false; } double r, g, b, a; if (!(in >> r >> g >> b >> a)) { - RCLCPP_ERROR(LOGGER, "Improperly formatted color in scene geometry file"); + RCLCPP_ERROR(getLogger(), "Improperly formatted color in scene geometry file"); return false; } if (shape) @@ -1081,7 +1089,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet in >> subframe_name; if (!utilities::readPoseFromText(in, pose)) { - RCLCPP_ERROR(LOGGER, "Failed to read subframe pose from scene file"); + RCLCPP_ERROR(getLogger(), "Failed to read subframe pose from scene file"); return false; } subframes[subframe_name] = pose; @@ -1096,7 +1104,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } else { - RCLCPP_ERROR(LOGGER, "Unknown marker in scene geometry file: %s ", marker.c_str()); + RCLCPP_ERROR(getLogger(), "Unknown marker in scene geometry file: %s ", marker.c_str()); return false; } } while (true); @@ -1125,7 +1133,7 @@ void PlanningScene::setCurrentState(const moveit_msgs::msg::RobotState& state) { if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::msg::CollisionObject::ADD) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "The specified RobotState is not marked as is_diff. " "The request to modify the object '%s' is not supported. Object is ignored.", state.attached_collision_objects[i].object.id.c_str()); @@ -1204,13 +1212,13 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen { bool result = true; - RCLCPP_DEBUG(LOGGER, "Adding planning scene diff"); + RCLCPP_DEBUG(getLogger(), "Adding planning scene diff"); if (!scene_msg.name.empty()) name_ = scene_msg.name; if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName()) { - RCLCPP_WARN(LOGGER, "Setting the scene for model '%s' but model '%s' is loaded.", + RCLCPP_WARN(getLogger(), "Setting the scene for model '%s' but model '%s' is loaded.", scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str()); } @@ -1255,12 +1263,12 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene_msg) { - RCLCPP_DEBUG(LOGGER, "Setting new planning scene: '%s'", scene_msg.name.c_str()); + RCLCPP_DEBUG(getLogger(), "Setting new planning scene: '%s'", scene_msg.name.c_str()); name_ = scene_msg.name; if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName()) { - RCLCPP_WARN(LOGGER, "Setting the scene for model '%s' but model '%s' is loaded.", + RCLCPP_WARN(getLogger(), "Setting the scene for model '%s' but model '%s' is loaded.", scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str()); } @@ -1332,7 +1340,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::Octomap& map) if (map.id != "OcTree") { - RCLCPP_ERROR(LOGGER, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str()); + RCLCPP_ERROR(getLogger(), "Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str()); return; } @@ -1372,7 +1380,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& if (map.octomap.id != "OcTree") { - RCLCPP_ERROR(LOGGER, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str()); + RCLCPP_ERROR(getLogger(), "Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str()); return; } @@ -1425,13 +1433,13 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At if (object.object.operation == moveit_msgs::msg::CollisionObject::ADD && !getRobotModel()->hasLinkModel(object.link_name)) { - RCLCPP_ERROR(LOGGER, "Unable to attach a body to link '%s' (link not found)", object.link_name.c_str()); + RCLCPP_ERROR(getLogger(), "Unable to attach a body to link '%s' (link not found)", object.link_name.c_str()); return false; } if (object.object.id == OCTOMAP_NS) { - RCLCPP_ERROR(LOGGER, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); + RCLCPP_ERROR(getLogger(), "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); return false; } @@ -1468,7 +1476,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At { if (obj_in_world) { - RCLCPP_DEBUG(LOGGER, "Attaching world object '%s' to link '%s'", object.object.id.c_str(), + RCLCPP_DEBUG(getLogger(), "Attaching world object '%s' to link '%s'", object.object.id.c_str(), object.link_name.c_str()); object_pose_in_link = robot_state_->getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_; @@ -1478,7 +1486,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At } else { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Attempting to attach object '%s' to link '%s' but no geometry specified " "and such an object does not exist in the collision world", object.object.id.c_str(), object.link_name.c_str()); @@ -1506,7 +1514,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At if (shapes.empty()) { - RCLCPP_ERROR(LOGGER, "There is no geometry to attach to link '%s' as part of attached body '%s'", + RCLCPP_ERROR(getLogger(), "There is no geometry to attach to link '%s' as part of attached body '%s'", object.link_name.c_str(), object.object.id.c_str()); return false; } @@ -1519,12 +1527,12 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At { if (object.object.operation == moveit_msgs::msg::CollisionObject::ADD) { - RCLCPP_DEBUG(LOGGER, "Removing world object with the same name as newly attached object: '%s'", + RCLCPP_DEBUG(getLogger(), "Removing world object with the same name as newly attached object: '%s'", object.object.id.c_str()); } else { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "You tried to append geometry to an attached object " "that is actually a world object ('%s'). World geometry is ignored.", object.object.id.c_str()); @@ -1537,14 +1545,15 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At { if (robot_state_->clearAttachedBody(object.object.id)) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "The robot state already had an object named '%s' attached to link '%s'. " "The object was replaced.", object.object.id.c_str(), object.link_name.c_str()); } robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, object.touch_links, object.link_name, object.detach_posture, subframe_poses); - RCLCPP_DEBUG(LOGGER, "Attached object '%s' to link '%s'", object.object.id.c_str(), object.link_name.c_str()); + RCLCPP_DEBUG(getLogger(), "Attached object '%s' to link '%s'", object.object.id.c_str(), + object.link_name.c_str()); } else // APPEND: augment to existing attached object { @@ -1567,14 +1576,14 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At robot_state_->clearAttachedBody(object.object.id); robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, touch_links, object.link_name, detach_posture, subframe_poses); - RCLCPP_DEBUG(LOGGER, "Appended things to object '%s' attached to link '%s'", object.object.id.c_str(), + RCLCPP_DEBUG(getLogger(), "Appended things to object '%s' attached to link '%s'", object.object.id.c_str(), object.link_name.c_str()); } return true; } else { - RCLCPP_ERROR(LOGGER, "Robot state is not compatible with robot model. This could be fatal."); + RCLCPP_ERROR(getLogger(), "Robot state is not compatible with robot model. This could be fatal."); } } else if (object.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) // == DETACH @@ -1601,10 +1610,10 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At { if (!object.link_name.empty() && (body->getAttachedLinkName() != object.link_name)) { - RCLCPP_ERROR_STREAM(LOGGER, "The AttachedCollisionObject message states the object is attached to " - << object.link_name << ", but it is actually attached to " - << body->getAttachedLinkName() - << ". Leave the link_name empty or specify the correct link."); + RCLCPP_ERROR_STREAM(getLogger(), "The AttachedCollisionObject message states the object is attached to " + << object.link_name << ", but it is actually attached to " + << body->getAttachedLinkName() + << ". Leave the link_name empty or specify the correct link."); return false; } attached_bodies.push_back(body); @@ -1617,7 +1626,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At const std::string& name = attached_body->getName(); if (world_->hasObject(name)) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "The collision world already has an object with the same name as the body about to be detached. " "NOT adding the detached body '%s' to the collision world.", object.object.id.c_str()); @@ -1636,7 +1645,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At setObjectColor(attached_body->getName(), original_object_color.value()); } - RCLCPP_DEBUG(LOGGER, "Detached object '%s' from link '%s' and added it back in the collision world", + RCLCPP_DEBUG(getLogger(), "Detached object '%s' from link '%s' and added it back in the collision world", name.c_str(), object.link_name.c_str()); } @@ -1647,11 +1656,11 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At } else if (object.object.operation == moveit_msgs::msg::CollisionObject::MOVE) { - RCLCPP_ERROR(LOGGER, "Move for attached objects not yet implemented"); + RCLCPP_ERROR(getLogger(), "Move for attached objects not yet implemented"); } else { - RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", object.object.operation); + RCLCPP_ERROR(getLogger(), "Unknown collision object operation: %d", object.object.operation); } return false; @@ -1661,7 +1670,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::msg::CollisionO { if (object.id == OCTOMAP_NS) { - RCLCPP_ERROR(LOGGER, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); + RCLCPP_ERROR(getLogger(), "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); return false; } @@ -1679,7 +1688,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::msg::CollisionO return processCollisionObjectMove(object); } - RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", object.operation); + RCLCPP_ERROR(getLogger(), "Unknown collision object operation: %d", object.operation); return false; } @@ -1690,17 +1699,17 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: { if (object.primitives.size() < object.primitive_poses.size()) { - RCLCPP_ERROR(LOGGER, "More primitive shape poses than shapes in collision object message."); + RCLCPP_ERROR(getLogger(), "More primitive shape poses than shapes in collision object message."); return false; } if (object.meshes.size() < object.mesh_poses.size()) { - RCLCPP_ERROR(LOGGER, "More mesh poses than meshes in collision object message."); + RCLCPP_ERROR(getLogger(), "More mesh poses than meshes in collision object message."); return false; } if (object.planes.size() < object.plane_poses.size()) { - RCLCPP_ERROR(LOGGER, "More plane poses than planes in collision object message."); + RCLCPP_ERROR(getLogger(), "More plane poses than planes in collision object message."); return false; } @@ -1743,9 +1752,9 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: const std::string& shape_type) { if (shape_vector.size() > shape_poses_vector.size()) { - RCLCPP_DEBUG_STREAM(LOGGER, "Number of " << shape_type - << " does not match number of poses " - "in collision object message. Assuming identity."); + RCLCPP_DEBUG_STREAM(getLogger(), "Number of " << shape_type + << " does not match number of poses " + "in collision object message. Assuming identity."); for (std::size_t i = 0; i < shape_vector.size(); ++i) { if (i >= shape_poses_vector.size()) @@ -1774,13 +1783,13 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::msg::CollisionO { if (!knowsFrameTransform(object.header.frame_id)) { - RCLCPP_ERROR_STREAM(LOGGER, "Unknown frame: " << object.header.frame_id); + RCLCPP_ERROR_STREAM(getLogger(), "Unknown frame: " << object.header.frame_id); return false; } if (object.primitives.empty() && object.meshes.empty() && object.planes.empty()) { - RCLCPP_ERROR(LOGGER, "There are no shapes specified in the collision object message"); + RCLCPP_ERROR(getLogger(), "There are no shapes specified in the collision object message"); return false; } @@ -1824,7 +1833,7 @@ bool PlanningScene::processCollisionObjectRemove(const moveit_msgs::msg::Collisi { if (!world_->removeObject(object.id)) { - RCLCPP_WARN_STREAM(LOGGER, + RCLCPP_WARN_STREAM(getLogger(), "Tried to remove world object '" << object.id << "', but it does not exist in this scene."); return false; } @@ -1841,7 +1850,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision { if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty()) { - RCLCPP_WARN(LOGGER, "Move operation for object '%s' ignores the geometry specified in the message.", + RCLCPP_WARN(getLogger(), "Move operation for object '%s' ignores the geometry specified in the message.", object.id.c_str()); } @@ -1855,7 +1864,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision return true; } - RCLCPP_ERROR(LOGGER, "World object '%s' does not exist. Cannot move.", object.id.c_str()); + RCLCPP_ERROR(getLogger(), "World object '%s' does not exist. Cannot move.", object.id.c_str()); return false; } @@ -2017,7 +2026,7 @@ void PlanningScene::setObjectColor(const std::string& object_id, const std_msgs: { if (object_id.empty()) { - RCLCPP_ERROR(LOGGER, "Cannot set color of object with empty object_id."); + RCLCPP_ERROR(getLogger(), "Cannot set color of object with empty object_id."); return; } if (!object_colors_) @@ -2256,7 +2265,7 @@ bool PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory& traject if (!found) { if (verbose) - RCLCPP_INFO(LOGGER, "Goal not satisfied"); + RCLCPP_INFO(getLogger(), "Goal not satisfied"); if (invalid_index) invalid_index->push_back(i); result = false; diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 924c094748..0d11decc6f 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -30,6 +30,7 @@ ament_target_dependencies(moveit_robot_model target_link_libraries(moveit_robot_model moveit_exceptions moveit_macros + moveit_utils ) if(BUILD_TESTING) diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 2c7f60b8d2..565533cedd 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -42,16 +42,22 @@ #include #include #include +#include namespace moveit { namespace core { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.floating_joint_model"); namespace { constexpr size_t STATE_SPACE_DIMENSION = 7; +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("floating_joint_model"); + return logger; +} + } // namespace FloatingJointModel::FloatingJointModel(const std::string& name, size_t joint_index, size_t first_variable_index) @@ -180,7 +186,7 @@ bool FloatingJointModel::normalizeRotation(double* values) const double norm = sqrt(norm_sqr); if (norm < std::numeric_limits::epsilon() * 100.0) { - RCLCPP_WARN(LOGGER, "Quaternion is zero in RobotState representation. Setting to identity"); + RCLCPP_WARN(getLogger(), "Quaternion is zero in RobotState representation. Setting to identity"); values[3] = 0.0; values[4] = 0.0; values[5] = 0.0; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 9779b0585a..6f611e963b 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "order_robot_model_items.inc" @@ -51,7 +52,11 @@ namespace core { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.joint_model_group"); +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("joint_model_group"); + return logger; +} // check if a parent or ancestor of joint is included in this group bool includesParent(const JointModel* joint, const JointModelGroup* group) @@ -316,7 +321,7 @@ const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const LinkModelMapConst::const_iterator it = link_model_map_.find(name); if (it == link_model_map_.end()) { - RCLCPP_ERROR(LOGGER, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); + RCLCPP_ERROR(getLogger(), "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -327,7 +332,7 @@ const JointModel* JointModelGroup::getJointModel(const std::string& name) const JointModelMapConst::const_iterator it = joint_model_map_.find(name); if (it == joint_model_map_.end()) { - RCLCPP_ERROR(LOGGER, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); + RCLCPP_ERROR(getLogger(), "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -377,7 +382,7 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum } else { - RCLCPP_WARN(LOGGER, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str()); + RCLCPP_WARN(getLogger(), "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str()); } active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], @@ -548,7 +553,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const JointModelGroup* eef = parent_model_->getEndEffector(name); if (!eef) { - RCLCPP_ERROR(LOGGER, "Unable to find joint model group for eef"); + RCLCPP_ERROR(getLogger(), "Unable to find joint model group for eef"); return false; } const std::string& eef_parent = eef->getEndEffectorParentGroup().second; @@ -556,7 +561,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent); if (!eef_link) { - RCLCPP_ERROR(LOGGER, "Unable to find end effector link for eef"); + RCLCPP_ERROR(getLogger(), "Unable to find end effector link for eef"); return false; } // insert eef_link into tips, maintaining a *sorted* vector, thus enabling use of std::lower_bound @@ -577,11 +582,11 @@ const LinkModel* JointModelGroup::getOnlyOneEndEffectorTip() const } else if (tips.size() > 1) { - RCLCPP_ERROR(LOGGER, "More than one end effector tip found for joint model group, so cannot return only one"); + RCLCPP_ERROR(getLogger(), "More than one end effector tip found for joint model group, so cannot return only one"); } else { - RCLCPP_ERROR(LOGGER, "No end effector tips found in joint model group"); + RCLCPP_ERROR(getLogger(), "No end effector tips found in joint model group"); } return nullptr; } @@ -591,7 +596,7 @@ int JointModelGroup::getVariableGroupIndex(const std::string& variable) const VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable); if (it == joint_variables_index_map_.end()) { - RCLCPP_ERROR(LOGGER, "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); + RCLCPP_ERROR(getLogger(), "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); return -1; } return it->second; @@ -618,7 +623,7 @@ bool JointModelGroup::computeJointVariableIndices(const std::vector // skip reported fixed joints if (hasJointModel(joint_name) && getJointModel(joint_name)->getType() == JointModel::FIXED) continue; - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Looking for variables for joint '%s', " "but group '%s' does not contain such a joint.", joint_name.c_str(), getName().c_str()); @@ -676,7 +681,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const if (tip_frames.empty()) { - RCLCPP_WARN(LOGGER, "Group %s has no tip frame(s)", name_.c_str()); + RCLCPP_WARN(getLogger(), "Group %s has no tip frame(s)", name_.c_str()); return false; } @@ -686,7 +691,8 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const // remove frame reference, if specified const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip; const std::string& tip_frame_local = tip_frame[0] == '/' ? tip_frame.substr(1) : tip_frame; - RCLCPP_DEBUG(LOGGER, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), tip_frame_local.c_str()); + RCLCPP_DEBUG(getLogger(), "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), + tip_frame_local.c_str()); // Check if the IK solver's tip is the same as the frame of inquiry if (tip_local != tip_frame_local) @@ -781,7 +787,7 @@ bool JointModelGroup::isValidVelocityMove(const std::vector& from_joint_ // Check for equal sized arrays if (from_joint_pose.size() != to_joint_pose.size()) { - RCLCPP_ERROR(LOGGER, "To and from joint poses are of different sizes."); + RCLCPP_ERROR(getLogger(), "To and from joint poses are of different sizes."); return false; } @@ -802,8 +808,8 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d if (var_bounds->size() != 1) { // TODO(davetcoleman) Support multiple variables - RCLCPP_ERROR(LOGGER, "Attempting to check velocity bounds for waypoint move with joints that have multiple " - "variables"); + RCLCPP_ERROR(getLogger(), "Attempting to check velocity bounds for waypoint move with joints that have multiple " + "variables"); return false; } const double max_velocity = (*var_bounds)[0].max_velocity_; @@ -811,7 +817,7 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d double max_dtheta = dt * max_velocity; if (dtheta > max_dtheta) { - RCLCPP_DEBUG(LOGGER, "Not valid velocity move because of joint %lu", i); + RCLCPP_DEBUG(getLogger(), "Not valid velocity move because of joint %lu", i); return false; } } diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index b68ac8ba49..828fb8c10e 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "order_robot_model_items.inc" @@ -49,7 +50,14 @@ namespace moveit { namespace core { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.robot_model"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("robot_model"); + return logger; +} +} // namespace RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model) { @@ -86,34 +94,34 @@ void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf:: link_geometry_count_ = 0; variable_count_ = 0; model_name_ = urdf_model.getName(); - RCLCPP_INFO(LOGGER, "Loading robot model '%s'...", model_name_.c_str()); + RCLCPP_INFO(getLogger(), "Loading robot model '%s'...", model_name_.c_str()); if (urdf_model.getRoot()) { const urdf::Link* root_link_ptr = urdf_model.getRoot().get(); model_frame_ = root_link_ptr->name; - RCLCPP_DEBUG(LOGGER, "... building kinematic chain"); + RCLCPP_DEBUG(getLogger(), "... building kinematic chain"); root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model); if (root_joint_) root_link_ = root_joint_->getChildLinkModel(); - RCLCPP_DEBUG(LOGGER, "... building mimic joints"); + RCLCPP_DEBUG(getLogger(), "... building mimic joints"); buildMimic(urdf_model); - RCLCPP_DEBUG(LOGGER, "... computing joint indexing"); + RCLCPP_DEBUG(getLogger(), "... computing joint indexing"); buildJointInfo(); if (link_models_with_collision_geometry_vector_.empty()) { - RCLCPP_WARN(LOGGER, "No geometry is associated to any robot links"); + RCLCPP_WARN(getLogger(), "No geometry is associated to any robot links"); } // build groups - RCLCPP_DEBUG(LOGGER, "... constructing joint groups"); + RCLCPP_DEBUG(getLogger(), "... constructing joint groups"); buildGroups(srdf_model); - RCLCPP_DEBUG(LOGGER, "... constructing joint group states"); + RCLCPP_DEBUG(getLogger(), "... constructing joint group states"); buildGroupStates(srdf_model); // For debugging entire model @@ -121,7 +129,7 @@ void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf:: } else { - RCLCPP_WARN(LOGGER, "No root link found"); + RCLCPP_WARN(getLogger(), "No root link found"); } } @@ -354,7 +362,7 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) } else { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "The model for joint '%s' requires %d variable values, " "but only %d variable values were supplied in default state '%s' for group '%s'", jt->first.c_str(), static_cast(vn.size()), static_cast(jt->second.size()), @@ -363,7 +371,7 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) } else { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Group state '%s' specifies value for joint '%s', " "but that joint is not part of group '%s'", group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); @@ -377,17 +385,17 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) { missing << ", " << (*j)->getName(); } - RCLCPP_WARN_STREAM(LOGGER, "Group state '" << group_state.name_ - << "' doesn't specify all group joints in group '" - << group_state.group_ << "'. " << missing.str() << ' ' - << (remaining_joints.size() > 1 ? "are" : "is") << " missing."); + RCLCPP_WARN_STREAM(getLogger(), "Group state '" << group_state.name_ + << "' doesn't specify all group joints in group '" + << group_state.group_ << "'. " << missing.str() << ' ' + << (remaining_joints.size() > 1 ? "are" : "is") << " missing."); } if (!state.empty()) jmg->addDefaultState(group_state.name_, state); } else { - RCLCPP_ERROR(LOGGER, "Group state '%s' specified for group '%s', but that group does not exist", + RCLCPP_ERROR(getLogger(), "Group state '%s' specified for group '%s', but that group does not exist", group_state.name_.c_str(), group_state.group_.c_str()); } } @@ -412,13 +420,13 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) } else { - RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic joint '%s' because they have different number of DOF", + RCLCPP_ERROR(getLogger(), "Joint '%s' cannot mimic joint '%s' because they have different number of DOF", joint_model->getName().c_str(), jm->mimic->joint_name.c_str()); } } else { - RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic unknown joint '%s'", joint_model->getName().c_str(), + RCLCPP_ERROR(getLogger(), "Joint '%s' cannot mimic unknown joint '%s'", joint_model->getName().c_str(), jm->mimic->joint_name.c_str()); } } @@ -444,7 +452,7 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) } if (joint_model == joint_model->getMimic()) { - RCLCPP_ERROR(LOGGER, "Cycle found in joint that mimic each other. Ignoring all mimic joints."); + RCLCPP_ERROR(getLogger(), "Cycle found in joint that mimic each other. Ignoring all mimic joints."); for (JointModel* joint_model_recal : joint_model_vector_) joint_model_recal->setMimic(nullptr, 0.0, 0.0); change = false; @@ -477,7 +485,7 @@ const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - RCLCPP_ERROR(LOGGER, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -491,7 +499,7 @@ JointModelGroup* RobotModel::getEndEffector(const std::string& name) it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - RCLCPP_ERROR(LOGGER, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -507,7 +515,7 @@ const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) c JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - RCLCPP_ERROR(LOGGER, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -518,7 +526,7 @@ JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - RCLCPP_ERROR(LOGGER, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -557,7 +565,7 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) processed[i] = true; if (!addJointModelGroup(group_configs[i])) { - RCLCPP_WARN(LOGGER, "Failed to add group '%s'", group_configs[i].name_.c_str()); + RCLCPP_WARN(getLogger(), "Failed to add group '%s'", group_configs[i].name_.c_str()); } } } @@ -568,7 +576,7 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) { if (!processed[i]) { - RCLCPP_WARN(LOGGER, "Could not process group '%s' due to unmet subgroup dependencies", + RCLCPP_WARN(getLogger(), "Could not process group '%s' due to unmet subgroup dependencies", group_configs[i].name_.c_str()); } } @@ -666,13 +674,13 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) } else { - RCLCPP_ERROR(LOGGER, "Group '%s' for end-effector '%s' cannot be its own parent", + RCLCPP_ERROR(getLogger(), "Group '%s' for end-effector '%s' cannot be its own parent", eef.parent_group_.c_str(), eef.name_.c_str()); } } else { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Group '%s' was specified as parent group for end-effector '%s' " "but it does not include the parent link '%s'", eef.parent_group_.c_str(), eef.name_.c_str(), eef.parent_link_.c_str()); @@ -680,7 +688,7 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) } else { - RCLCPP_ERROR(LOGGER, "Group name '%s' not found (specified as parent group for end-effector '%s')", + RCLCPP_ERROR(getLogger(), "Group name '%s' not found (specified as parent group for end-effector '%s')", eef.parent_group_.c_str(), eef.name_.c_str()); } } @@ -709,7 +717,7 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) } else { - RCLCPP_WARN(LOGGER, "Could not identify parent group for end-effector '%s'", eef.name_.c_str()); + RCLCPP_WARN(getLogger(), "Could not identify parent group for end-effector '%s'", eef.name_.c_str()); it->second->setEndEffectorParent("", eef.parent_link_); } } @@ -722,7 +730,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) { if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end()) { - RCLCPP_WARN(LOGGER, "A group named '%s' already exists. Not adding.", gc.name_.c_str()); + RCLCPP_WARN(getLogger(), "A group named '%s' already exists. Not adding.", gc.name_.c_str()); return false; } @@ -823,7 +831,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) if (jset.empty()) { - RCLCPP_WARN(LOGGER, "Group '%s' must have at least one valid joint", gc.name_.c_str()); + RCLCPP_WARN(getLogger(), "Group '%s' must have at least one valid joint", gc.name_.c_str()); return false; } @@ -970,7 +978,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const new_joint_model = new FixedJointModel(parent_joint->name, joint_index, first_variable_index); break; default: - RCLCPP_ERROR(LOGGER, "Unknown joint type: %d", static_cast(parent_joint->type)); + RCLCPP_ERROR(getLogger(), "Unknown joint type: %d", static_cast(parent_joint->type)); break; } } @@ -990,7 +998,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const } else { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Skipping virtual joint '%s' because its child frame '%s' " "does not match the URDF frame '%s'", virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str()); @@ -998,7 +1006,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const } else if (virtual_joint.parent_frame_.empty()) { - RCLCPP_WARN(LOGGER, "Skipping virtual joint '%s' because its parent frame is empty", + RCLCPP_WARN(getLogger(), "Skipping virtual joint '%s' because its parent frame is empty", virtual_joint.name_.c_str()); } else @@ -1028,7 +1036,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const } if (!new_joint_model) { - RCLCPP_INFO(LOGGER, "No root/virtual joint specified in SRDF. Assuming fixed joint"); + RCLCPP_INFO(getLogger(), "No root/virtual joint specified in SRDF. Assuming fixed joint"); new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT", joint_index, first_variable_index); } } @@ -1057,16 +1065,16 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const angular_distance_weight = std::stod(property.value_, &sz); if (sz != property.value_.size()) { - RCLCPP_WARN_STREAM(LOGGER, "Extra characters after property " << property.property_name_ << " for joint " - << property.joint_name_ << " as double: '" - << property.value_.substr(sz) << '\''); + RCLCPP_WARN_STREAM(getLogger(), "Extra characters after property " + << property.property_name_ << " for joint " << property.joint_name_ + << " as double: '" << property.value_.substr(sz) << '\''); } } catch (const std::invalid_argument& e) { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to parse property " << property.property_name_ << " for joint " - << property.joint_name_ << " as double: '" - << property.value_ << '\''); + RCLCPP_ERROR_STREAM(getLogger(), "Unable to parse property " << property.property_name_ << " for joint " + << property.joint_name_ << " as double: '" + << property.value_ << '\''); continue; } @@ -1080,15 +1088,15 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const } else { - RCLCPP_ERROR_STREAM(LOGGER, "Cannot apply property " << property.property_name_ - << " to joint type: " << new_joint_model->getTypeName()); + RCLCPP_ERROR_STREAM(getLogger(), "Cannot apply property " << property.property_name_ << " to joint type: " + << new_joint_model->getTypeName()); } } else if (property.property_name_ == "motion_model") { if (new_joint_model->getType() != JointModel::JointType::PLANAR) { - RCLCPP_ERROR(LOGGER, "Cannot apply property %s to joint type: %s", property.property_name_.c_str(), + RCLCPP_ERROR(getLogger(), "Cannot apply property %s to joint type: %s", property.property_name_.c_str(), new_joint_model->getTypeName().c_str()); continue; } @@ -1104,10 +1112,10 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const } else { - RCLCPP_ERROR_STREAM(LOGGER, "Unknown value for property " << property.property_name_ << " (" - << property.joint_name_ << "): '" << property.value_ - << '\''); - RCLCPP_ERROR(LOGGER, "Valid values are 'holonomic' and 'diff_drive'"); + RCLCPP_ERROR_STREAM(getLogger(), "Unknown value for property " << property.property_name_ << " (" + << property.joint_name_ << "): '" + << property.value_ << '\''); + RCLCPP_ERROR(getLogger(), "Valid values are 'holonomic' and 'diff_drive'"); continue; } @@ -1117,7 +1125,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const { if (new_joint_model->getType() != JointModel::JointType::PLANAR) { - RCLCPP_ERROR(LOGGER, "Cannot apply property %s to joint type: %s", property.property_name_.c_str(), + RCLCPP_ERROR(getLogger(), "Cannot apply property %s to joint type: %s", property.property_name_.c_str(), new_joint_model->getTypeName().c_str()); continue; } @@ -1128,16 +1136,16 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const min_translational_distance = std::stod(property.value_, &sz); if (sz != property.value_.size()) { - RCLCPP_WARN_STREAM(LOGGER, "Extra characters after property " << property.property_name_ << " for joint " - << property.joint_name_ << " as double: '" - << property.value_.substr(sz) << '\''); + RCLCPP_WARN_STREAM(getLogger(), "Extra characters after property " + << property.property_name_ << " for joint " << property.joint_name_ + << " as double: '" << property.value_.substr(sz) << '\''); } } catch (const std::invalid_argument& e) { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to parse property " << property.property_name_ << " for joint " - << property.joint_name_ << " as double: '" - << property.value_ << '\''); + RCLCPP_ERROR_STREAM(getLogger(), "Unable to parse property " << property.property_name_ << " for joint " + << property.joint_name_ << " as double: '" + << property.value_ << '\''); continue; } @@ -1145,7 +1153,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const } else { - RCLCPP_ERROR(LOGGER, "Unknown joint property: %s", property.property_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Unknown joint property: %s", property.property_name_.c_str()); } } } @@ -1202,7 +1210,7 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) } if (warn_about_missing_collision) { - RCLCPP_WARN_STREAM(LOGGER, // TODO(henningkayser): use child namespace "empty_collision_geometry" + RCLCPP_WARN_STREAM(getLogger(), // TODO(henningkayser): use child namespace "empty_collision_geometry" "Link " << urdf_link->name << " has visual geometry but no collision geometry. " "Collision geometry will be left empty. " @@ -1276,7 +1284,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) } break; default: - RCLCPP_ERROR(LOGGER, "Unknown geometry type: %d", static_cast(geom->type)); + RCLCPP_ERROR(getLogger(), "Unknown geometry type: %d", static_cast(geom->type)); break; } @@ -1298,7 +1306,7 @@ const JointModel* RobotModel::getJointModel(const std::string& name) const JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - RCLCPP_ERROR(LOGGER, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1306,7 +1314,7 @@ const JointModel* RobotModel::getJointModel(size_t index) const { if (index >= joint_model_vector_.size()) { - RCLCPP_ERROR(LOGGER, "Joint index '%li' out of bounds of joints in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Joint index '%li' out of bounds of joints in model '%s'", index, model_name_.c_str()); return nullptr; } assert(joint_model_vector_[index]->getJointIndex() == index); @@ -1318,7 +1326,7 @@ JointModel* RobotModel::getJointModel(const std::string& name) JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - RCLCPP_ERROR(LOGGER, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1331,7 +1339,7 @@ const LinkModel* RobotModel::getLinkModel(size_t index) const { if (index >= link_model_vector_.size()) { - RCLCPP_ERROR(LOGGER, "Link index '%li' out of bounds of links in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Link index '%li' out of bounds of links in model '%s'", index, model_name_.c_str()); return nullptr; } assert(link_model_vector_[index]->getLinkIndex() == index); @@ -1352,7 +1360,7 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link) } else { // Otherwise print error - RCLCPP_ERROR(LOGGER, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(getLogger(), "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); } return nullptr; } @@ -1492,7 +1500,7 @@ double RobotModel::distance(const double* state1, const double* state2) const void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const { - moveit::core::checkInterpolationParamBounds(LOGGER, t); + moveit::core::checkInterpolationParamBounds(getLogger(), t); // we interpolate values only for active joint models (non-mimic) for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) { @@ -1568,7 +1576,7 @@ void RobotModel::setKinematicsAllocators(const std::mapgetName() << ' '; solver_allocator_pair.second[sub] = allocators.find(sub->getName())->second; } - RCLCPP_DEBUG(LOGGER, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), + RCLCPP_DEBUG(getLogger(), "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str()); } jmg->setSolverAllocators(solver_allocator_pair); diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index b0550b96da..e99af0424d 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -42,6 +42,8 @@ #include #include #include +#include + namespace moveit::core { @@ -50,16 +52,21 @@ namespace moveit::core // will be printed out. static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.cartesian_interpolator"); - namespace { + +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("cartesian_interpolator"); + return logger; +} + std::optional hasRelativeJointSpaceJump(const std::vector& waypoints, const moveit::core::JointModelGroup& group, double jump_threshold_factor) { if (waypoints.size() < MIN_STEPS_FOR_JUMP_THRESH) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "The computed path is too short to detect jumps in joint-space. " "Need at least %zu steps, only got %zu. Try a lower max_step.", MIN_STEPS_FOR_JUMP_THRESH, waypoints.size()); @@ -116,7 +123,7 @@ std::optional hasAbsoluteJointSpaceJump(const std::vector #include #include +#include namespace moveit { namespace core { + // ******************************************** // * Internal (hidden) functions // ******************************************** - namespace { -// Logger -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.conversions"); +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("conversions"); + return logger; +} bool jointStateToRobotStateImpl(const sensor_msgs::msg::JointState& joint_state, RobotState& state) { if (joint_state.name.size() != joint_state.position.size()) { - RCLCPP_ERROR(LOGGER, "Different number of names and positions in JointState message: %zu, %zu", + RCLCPP_ERROR(getLogger(), "Different number of names and positions in JointState message: %zu, %zu", joint_state.name.size(), joint_state.position.size()); return false; } @@ -75,7 +79,7 @@ bool multiDofJointsToRobotState(const sensor_msgs::msg::MultiDOFJointState& mjs, std::size_t nj = mjs.joint_names.size(); if (nj != mjs.transforms.size()) { - RCLCPP_ERROR(LOGGER, "Different number of names, values or frames in MultiDOFJointState message."); + RCLCPP_ERROR(getLogger(), "Different number of names, values or frames in MultiDOFJointState message."); return false; } @@ -98,7 +102,7 @@ bool multiDofJointsToRobotState(const sensor_msgs::msg::MultiDOFJointState& mjs, } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Caught %s", ex.what()); + RCLCPP_ERROR(getLogger(), "Caught %s", ex.what()); error = true; } } @@ -109,7 +113,7 @@ bool multiDofJointsToRobotState(const sensor_msgs::msg::MultiDOFJointState& mjs, if (error) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "The transform for multi-dof joints was specified in frame '%s' " "but it was not possible to transform that to frame '%s'", mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); @@ -121,7 +125,7 @@ bool multiDofJointsToRobotState(const sensor_msgs::msg::MultiDOFJointState& mjs, const std::string& joint_name = mjs.joint_names[i]; if (!state.getRobotModel()->hasJointModel(joint_name)) { - RCLCPP_WARN(LOGGER, "No joint matching multi-dof joint '%s'", joint_name.c_str()); + RCLCPP_WARN(getLogger(), "No joint matching multi-dof joint '%s'", joint_name.c_str()); error = true; continue; } @@ -246,26 +250,26 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol { if (aco.object.primitives.size() != aco.object.primitive_poses.size()) { - RCLCPP_ERROR(LOGGER, "Number of primitive shapes does not match " - "number of poses in collision object message"); + RCLCPP_ERROR(getLogger(), "Number of primitive shapes does not match " + "number of poses in collision object message"); return; } if (aco.object.meshes.size() != aco.object.mesh_poses.size()) { - RCLCPP_ERROR(LOGGER, "Number of meshes does not match number of poses in collision object message"); + RCLCPP_ERROR(getLogger(), "Number of meshes does not match number of poses in collision object message"); return; } if (aco.object.planes.size() != aco.object.plane_poses.size()) { - RCLCPP_ERROR(LOGGER, "Number of planes does not match number of poses in collision object message"); + RCLCPP_ERROR(getLogger(), "Number of planes does not match number of poses in collision object message"); return; } if (aco.object.subframe_poses.size() != aco.object.subframe_names.size()) { - RCLCPP_ERROR(LOGGER, "Number of subframe poses does not match number of subframe names in message"); + RCLCPP_ERROR(getLogger(), "Number of subframe poses does not match number of subframe names in message"); return; } @@ -321,7 +325,7 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol else { world_to_header_frame.setIdentity(); - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Cannot properly transform from frame '%s'. " "The pose of the attached body may be incorrect", aco.object.header.frame_id.c_str()); @@ -332,34 +336,35 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol if (shapes.empty()) { - RCLCPP_ERROR(LOGGER, "There is no geometry to attach to link '%s' as part of attached body '%s'", + RCLCPP_ERROR(getLogger(), "There is no geometry to attach to link '%s' as part of attached body '%s'", aco.link_name.c_str(), aco.object.id.c_str()); } else { if (state.clearAttachedBody(aco.object.id)) { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "The robot state already had an object named '%s' attached to link '%s'. " "The object was replaced.", aco.object.id.c_str(), aco.link_name.c_str()); } state.attachBody(aco.object.id, object_pose, shapes, shape_poses, aco.touch_links, aco.link_name, aco.detach_posture, subframe_poses); - RCLCPP_DEBUG(LOGGER, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); + RCLCPP_DEBUG(getLogger(), "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); } } } else - RCLCPP_ERROR(LOGGER, "The attached body for link '%s' has no geometry", aco.link_name.c_str()); + RCLCPP_ERROR(getLogger(), "The attached body for link '%s' has no geometry", aco.link_name.c_str()); } else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { if (!state.clearAttachedBody(aco.object.id)) - RCLCPP_ERROR(LOGGER, "The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str()); + RCLCPP_ERROR(getLogger(), "The attached body '%s' can not be removed because it does not exist", + aco.link_name.c_str()); } else - RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", aco.object.operation); + RCLCPP_ERROR(getLogger(), "Unknown collision object operation: %d", aco.object.operation); } bool robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, @@ -370,7 +375,7 @@ bool robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::ms if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty()) { - RCLCPP_ERROR(LOGGER, "Found empty JointState message"); + RCLCPP_ERROR(getLogger(), "Found empty JointState message"); return false; } @@ -469,12 +474,12 @@ bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& tra { if (trajectory.points.empty() || point_id > trajectory.points.size() - 1) { - RCLCPP_ERROR(LOGGER, "Invalid point_id"); + RCLCPP_ERROR(getLogger(), "Invalid point_id"); return false; } if (trajectory.joint_names.empty()) { - RCLCPP_ERROR(LOGGER, "No joint names specified"); + RCLCPP_ERROR(getLogger(), "No joint names specified"); return false; } @@ -564,7 +569,7 @@ void streamToRobotState(RobotState& state, const std::string& line, const std::s { // Get a variable if (!std::getline(line_stream, cell, separator[0])) - RCLCPP_ERROR(LOGGER, "Missing variable %s", state.getVariableNames()[i].c_str()); + RCLCPP_ERROR(getLogger(), "Missing variable %s", state.getVariableNames()[i].c_str()); state.getVariablePositions()[i] = std::stod(cell); } } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 1a52f8a0da..206bf99e1d 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -49,13 +49,20 @@ #include #include #include +#include namespace moveit { namespace core { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.robot_state"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("robot_state"); + return logger; +} +} // namespace RobotState::RobotState(const RobotModelConstPtr& robot_model) : robot_model_(robot_model) @@ -131,7 +138,7 @@ bool RobotState::checkJointTransforms(const JointModel* joint) const { if (dirtyJointTransform(joint)) { - RCLCPP_WARN(LOGGER, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); + RCLCPP_WARN(getLogger(), "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); return false; } return true; @@ -141,7 +148,7 @@ bool RobotState::checkLinkTransforms() const { if (dirtyLinkTransforms()) { - RCLCPP_WARN(LOGGER, "Returning dirty link transforms"); + RCLCPP_WARN(getLogger(), "Returning dirty link transforms"); return false; } return true; @@ -151,7 +158,7 @@ bool RobotState::checkCollisionTransforms() const { if (dirtyCollisionBodyTransforms()) { - RCLCPP_WARN(LOGGER, "Returning dirty collision body transforms"); + RCLCPP_WARN(getLogger(), "Returning dirty collision body transforms"); return false; } return true; @@ -453,7 +460,7 @@ void RobotState::setJointEfforts(const JointModel* joint, const double* effort) { if (has_acceleration_) { - RCLCPP_ERROR(LOGGER, "Unable to set joint efforts because array is being used for accelerations"); + RCLCPP_ERROR(getLogger(), "Unable to set joint efforts because array is being used for accelerations"); return; } has_effort_ = true; @@ -957,7 +964,7 @@ double RobotState::distance(const RobotState& other, const JointModelGroup* join void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const { - checkInterpolationParamBounds(LOGGER, t); + checkInterpolationParamBounds(getLogger(), t); robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions()); std::fill(state.dirty_joint_transforms_.begin(), state.dirty_joint_transforms_.end(), 1); @@ -966,7 +973,7 @@ void RobotState::interpolate(const RobotState& to, double t, RobotState& state) void RobotState::interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const { - checkInterpolationParamBounds(LOGGER, t); + checkInterpolationParamBounds(getLogger(), t); const std::vector& jm = joint_group->getActiveJointModels(); for (const JointModel* joint : jm) { @@ -991,7 +998,7 @@ const AttachedBody* RobotState::getAttachedBody(const std::string& id) const const auto it = attached_body_map_.find(id); if (it == attached_body_map_.end()) { - RCLCPP_ERROR(LOGGER, "Attached body '%s' not found", id.c_str()); + RCLCPP_ERROR(getLogger(), "Attached body '%s' not found", id.c_str()); return nullptr; } else @@ -1119,7 +1126,7 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& frame_ } else if (!found) { - RCLCPP_WARN(LOGGER, "getFrameTransform() did not find a frame with name %s.", frame_id.c_str()); + RCLCPP_WARN(getLogger(), "getFrameTransform() did not find a frame with name %s.", frame_id.c_str()); } return result; @@ -1216,7 +1223,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons rclcpp::Clock clock; for (const std::string& link_name : link_names) { - RCLCPP_DEBUG(LOGGER, "Trying to get marker for link '%s'", link_name.c_str()); + RCLCPP_DEBUG(getLogger(), "Trying to get marker for link '%s'", link_name.c_str()); const LinkModel* link_model = robot_model_->getLinkModel(link_name); if (!link_model) continue; @@ -1301,14 +1308,14 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link // Check that the group is a chain, contains 'link' and has joint models. if (!group->isChain()) { - RCLCPP_ERROR(LOGGER, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); + RCLCPP_ERROR(getLogger(), "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); return false; } const std::set& descendant_links = group->getUpdatedLinkModelsSet(); if (descendant_links.find(link) == descendant_links.end()) { - RCLCPP_ERROR(LOGGER, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", + RCLCPP_ERROR(getLogger(), "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", link->getName().c_str(), group->getName().c_str()); return false; } @@ -1316,7 +1323,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link const std::vector& joint_models = group->getActiveJointModels(); if (joint_models.empty()) { - RCLCPP_ERROR(LOGGER, "The group '%s' doesn't contain any joint models. Cannot compute Jacobian.", + RCLCPP_ERROR(getLogger(), "The group '%s' doesn't contain any joint models. Cannot compute Jacobian.", group->getName().c_str()); return false; } @@ -1371,7 +1378,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link } else { - RCLCPP_ERROR(LOGGER, "Unknown type of joint in Jacobian computation"); + RCLCPP_ERROR(getLogger(), "Unknown type of joint in Jacobian computation"); return false; } i += joint_model->getVariableCount(); @@ -1483,7 +1490,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg: const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(getLogger(), "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options, cost_function); @@ -1508,7 +1515,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(getLogger(), "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } static std::vector consistency_limits; @@ -1560,7 +1567,7 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); if (!link_model) { - RCLCPP_ERROR(LOGGER, "The following IK frame does not exist: %s", ik_frame.c_str()); + RCLCPP_ERROR(getLogger(), "The following IK frame does not exist: %s", ik_frame.c_str()); return false; } pose = getGlobalLinkTransform(link_model).inverse() * pose; @@ -1607,7 +1614,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Error check if (poses_in.size() != tips_in.size()) { - RCLCPP_ERROR(LOGGER, "Number of poses must be the same as number of tips"); + RCLCPP_ERROR(getLogger(), "Number of poses must be the same as number of tips"); return false; } @@ -1627,7 +1634,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (!solver->supportsGroup(jmg, &error_msg)) { const kinematics::KinematicsBase& solver_ref = *solver; - RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s", + RCLCPP_ERROR(getLogger(), "Kinematics solver %s does not support joint group %s. Error: %s", typeid(solver_ref).name(), jmg->getName().c_str(), error_msg.c_str()); valid_solver = false; } @@ -1643,7 +1650,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } else { - RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(getLogger(), "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } } @@ -1652,7 +1659,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::vector consistency_limits; if (consistency_limit_sets.size() > 1) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Invalid number (%zu) of sets of consistency limits for a setFromIK request " "that is being solved by a single IK solver", consistency_limit_sets.size()); @@ -1715,7 +1722,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is const LinkModel* link_model = getLinkModel(pose_frame); if (!link_model) { - RCLCPP_ERROR(LOGGER, "The following Pose Frame does not exist: %s", pose_frame.c_str()); + RCLCPP_ERROR(getLogger(), "The following Pose Frame does not exist: %s", pose_frame.c_str()); return false; } const LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); @@ -1744,12 +1751,12 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Make sure one of the tip frames worked if (!found_valid_frame) { - RCLCPP_ERROR(LOGGER, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); + RCLCPP_ERROR(getLogger(), "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); // Debug available tip frames std::stringstream ss; for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id) ss << solver_tip_frames[solver_tip_id] << ", "; - RCLCPP_ERROR(LOGGER, "Available tip frames: [%s]", ss.str().c_str()); + RCLCPP_ERROR(getLogger(), "Available tip frames: [%s]", ss.str().c_str()); return false; } @@ -1852,21 +1859,21 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // Error check if (poses_in.size() != sub_groups.size()) { - RCLCPP_ERROR(LOGGER, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), + RCLCPP_ERROR(getLogger(), "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), sub_groups.size()); return false; } if (tips_in.size() != sub_groups.size()) { - RCLCPP_ERROR(LOGGER, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), + RCLCPP_ERROR(getLogger(), "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), sub_groups.size()); return false; } if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size()) { - RCLCPP_ERROR(LOGGER, "Number of consistency limit vectors must be the same as number of sub-groups"); + RCLCPP_ERROR(getLogger(), "Number of consistency limit vectors must be the same as number of sub-groups"); return false; } @@ -1874,7 +1881,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: { if (consistency_limits[i].size() != sub_groups[i]->getVariableCount()) { - RCLCPP_ERROR(LOGGER, "Number of joints in consistency_limits is %zu but it should be should be %u", i, + RCLCPP_ERROR(getLogger(), "Number of joints in consistency_limits is %zu but it should be should be %u", i, sub_groups[i]->getVariableCount()); return false; } @@ -1887,7 +1894,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance(); if (!solver) { - RCLCPP_ERROR(LOGGER, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); + RCLCPP_ERROR(getLogger(), "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); return false; } solvers.push_back(solver); @@ -1945,8 +1952,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (pose_frame != solver_tip_frame) { - RCLCPP_ERROR(LOGGER, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(), - solver_tip_frame.c_str()); + RCLCPP_ERROR(getLogger(), "Cannot compute IK for query pose reference frame '%s', desired: '%s'", + pose_frame.c_str(), solver_tip_frame.c_str()); return false; } } @@ -1987,7 +1994,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: do { ++attempts; - RCLCPP_DEBUG(LOGGER, "IK attempt: %d", attempts); + RCLCPP_DEBUG(getLogger(), "IK attempt: %d", attempts); bool found_solution = true; for (std::size_t sg = 0; sg < sub_groups.size(); ++sg) { @@ -2035,7 +2042,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: copyJointGroupPositions(jmg, full_solution); if (constraint ? constraint(this, jmg, &full_solution[0]) : true) { - RCLCPP_DEBUG(LOGGER, "Found IK solution"); + RCLCPP_DEBUG(getLogger(), "Found IK solution"); return true; } } diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index d571c8cab2..5dcff268a8 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -13,6 +13,7 @@ ament_target_dependencies(moveit_robot_trajectory target_link_libraries(moveit_robot_trajectory moveit_robot_model moveit_robot_state + moveit_utils ) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 2fe07f1e30..1e73da14f4 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -44,9 +44,19 @@ #include #include #include +#include namespace robot_trajectory { +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("robot_trajectory"); + return logger; +} +} // namespace + RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model) : robot_model_(robot_model), group_(nullptr) { @@ -93,7 +103,7 @@ double RobotTrajectory::getAverageSegmentDuration() const { if (duration_from_previous_.empty()) { - RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "Too few waypoints to calculate a duration. Returning 0."); + RCLCPP_WARN(getLogger(), "Too few waypoints to calculate a duration. Returning 0."); return 0.0; } @@ -102,7 +112,7 @@ double RobotTrajectory::getAverageSegmentDuration() const { if (duration_from_previous_.size() <= 1) { - RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "First and only waypoint has a duration of 0."); + RCLCPP_WARN(getLogger(), "First and only waypoint has a duration of 0."); return 0.0; } else diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 4a22a813ce..eeb791e8ab 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -63,6 +63,8 @@ class RobotTrajectoryTestFixture : public testing::Test void TearDown() override { + robot_model_.reset(); + robot_state_.reset(); } void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index dfd3b6d029..7b86373886 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -41,12 +41,12 @@ #include #include #include +#include namespace trajectory_processing { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing"); constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 constexpr double DEFAULT_MAX_JERK = 1000; // rad/s^3 @@ -54,6 +54,12 @@ constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0; constexpr double DURATION_EXTENSION_FRACTION = 1.1; // If "mitigate_overshoot" is enabled, overshoot is checked with this timestep constexpr double OVERSHOOT_CHECK_PERIOD = 0.01; // sec + +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("ruckig_traj_smoothing"); + return logger; +} } // namespace bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, @@ -69,7 +75,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto const size_t num_waypoints = trajectory.getWayPointCount(); if (num_waypoints < 2) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory."); return true; } @@ -80,7 +86,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto ruckig::InputParameter ruckig_input{ num_dof }; if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input)) { - RCLCPP_ERROR(LOGGER, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel."); + RCLCPP_ERROR(getLogger(), "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel."); return false; } @@ -103,7 +109,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto const size_t num_waypoints = trajectory.getWayPointCount(); if (num_waypoints < 2) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory."); return true; } @@ -114,7 +120,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto ruckig::InputParameter ruckig_input{ num_dof }; if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input)) { - RCLCPP_ERROR(LOGGER, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel."); + RCLCPP_ERROR(getLogger(), "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel."); return false; } @@ -179,7 +185,7 @@ bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& tra const moveit::core::JointModelGroup* const group = trajectory.getGroup(); if (!group) { - RCLCPP_ERROR(LOGGER, "The planner did not set the group the plan was computed for"); + RCLCPP_ERROR(getLogger(), "The planner did not set the group the plan was computed for"); return false; } return true; @@ -204,7 +210,7 @@ bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_fact } else { - RCLCPP_WARN_STREAM_ONCE(LOGGER, + RCLCPP_WARN_STREAM_ONCE(getLogger(), "Joint velocity limits are not defined. Using the default " << DEFAULT_MAX_VELOCITY << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml."); @@ -216,7 +222,7 @@ bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_fact } else { - RCLCPP_WARN_STREAM_ONCE(LOGGER, + RCLCPP_WARN_STREAM_ONCE(getLogger(), "Joint acceleration limits are not defined. Using the default " << DEFAULT_MAX_ACCELERATION << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml."); @@ -229,9 +235,9 @@ bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_fact } else { - RCLCPP_WARN_STREAM_ONCE(LOGGER, "Joint jerk limits are not defined. Using the default " - << DEFAULT_MAX_JERK - << " rad/s^3. You can define jerk limits in joint_limits.yaml."); + RCLCPP_WARN_STREAM_ONCE(getLogger(), "Joint jerk limits are not defined. Using the default " + << DEFAULT_MAX_JERK + << " rad/s^3. You can define jerk limits in joint_limits.yaml."); ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK; } } @@ -316,7 +322,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished) { - RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result); + RCLCPP_ERROR_STREAM(getLogger(), "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result); return false; } diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 9a0319024b..662f76087f 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -44,15 +44,21 @@ #include #include #include +#include namespace trajectory_processing { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation"); constexpr double DEFAULT_TIMESTEP = 1e-3; constexpr double EPS = 1e-6; constexpr double DEFAULT_SCALING_FACTOR = 1.0; + +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("time_optimal_trajectory_generation"); + return logger; +} } // namespace class LinearPathSegment : public PathSegment @@ -327,7 +333,7 @@ Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, co if (time_step_ == 0) { valid_ = false; - RCLCPP_ERROR(LOGGER, "The trajectory is invalid because the time step is 0."); + RCLCPP_ERROR(getLogger(), "The trajectory is invalid because the time step is 0."); return; } trajectory_.push_back(TrajectoryStep(0.0, 0.0)); @@ -557,7 +563,7 @@ bool Trajectory::integrateForward(std::list& trajectory, double else if (path_vel < 0.0) { valid_ = false; - RCLCPP_ERROR(LOGGER, "Error while integrating forward: Negative path velocity"); + RCLCPP_ERROR(getLogger(), "Error while integrating forward: Negative path velocity"); return true; } @@ -576,8 +582,8 @@ bool Trajectory::integrateForward(std::list& trajectory, double // The position will never change if velocity and acceleration are zero. // The loop will spin indefinitely as no exit condition is met. valid_ = false; - RCLCPP_ERROR(LOGGER, "Error while integrating forward: zero acceleration and velocity. Are any relevant " - "acceleration components limited to zero?"); + RCLCPP_ERROR(getLogger(), "Error while integrating forward: zero acceleration and velocity. Are any relevant " + "acceleration components limited to zero?"); return true; } @@ -664,7 +670,7 @@ void Trajectory::integrateBackward(std::list& start_trajectory, if (path_vel < 0.0) { valid_ = false; - RCLCPP_ERROR(LOGGER, "Error while integrating backward: Negative path velocity"); + RCLCPP_ERROR(getLogger(), "Error while integrating backward: Negative path velocity"); end_trajectory_ = trajectory; return; } @@ -693,7 +699,7 @@ void Trajectory::integrateBackward(std::list& start_trajectory, } valid_ = false; - RCLCPP_ERROR(LOGGER, "Error while integrating backward: Did not hit start trajectory"); + RCLCPP_ERROR(getLogger(), "Error while integrating backward: Did not hit start trajectory"); end_trajectory_ = trajectory; } @@ -889,7 +895,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { - RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for"); + RCLCPP_ERROR(getLogger(), "It looks like the planner did not set the group the plan was computed for"); return false; } @@ -903,7 +909,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT std::vector active_joint_indices; if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), active_joint_indices)) { - RCLCPP_ERROR(LOGGER, "Failed to get active variable indices."); + RCLCPP_ERROR(getLogger(), "Failed to get active variable indices."); } const size_t num_active_joints = active_joint_indices.size(); @@ -919,7 +925,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT { if (bounds.max_velocity_ <= 0.0) { - RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", + RCLCPP_ERROR(getLogger(), "Invalid max_velocity %f specified for '%s', must be greater than 0.0", bounds.max_velocity_, vars[idx].c_str()); return false; } @@ -928,9 +934,9 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } else { - RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint " - << vars[idx].c_str() - << "! You have to define velocity limits in the URDF or joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " + << vars[idx].c_str() + << "! You have to define velocity limits in the URDF or joint_limits.yaml"); return false; } @@ -938,7 +944,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT { if (bounds.max_acceleration_ < 0.0) { - RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", + RCLCPP_ERROR(getLogger(), "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", bounds.max_acceleration_, vars[idx].c_str()); return false; } @@ -947,10 +953,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } else { - RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint " - << vars[idx].c_str() - << "! You have to define acceleration limits in the URDF or " - "joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " + << vars[idx].c_str() + << "! You have to define acceleration limits in the URDF or " + "joint_limits.yaml"); return false; } } @@ -993,7 +999,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { - RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for"); + RCLCPP_ERROR(getLogger(), "It looks like the planner did not set the group the plan was computed for"); return false; } @@ -1007,7 +1013,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( std::vector indices; if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), indices)) { - RCLCPP_ERROR(LOGGER, "Failed to get active variable indices."); + RCLCPP_ERROR(getLogger(), "Failed to get active variable indices."); } const size_t num_joints = indices.size(); @@ -1033,7 +1039,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( // Set the default velocity limit, from robot model if (bounds.max_velocity_ < 0.0) { - RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", + RCLCPP_ERROR(getLogger(), "Invalid max_velocity %f specified for '%s', must be greater than 0.0", bounds.max_velocity_, vars[idx].c_str()); return false; } @@ -1044,10 +1050,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( if (!set_velocity_limit) { - RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint " - << vars[idx].c_str() - << "! You have to define velocity limits in the URDF or " - "joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " + << vars[idx].c_str() + << "! You have to define velocity limits in the URDF or " + "joint_limits.yaml"); return false; } @@ -1066,7 +1072,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( // Set the default acceleration limit, from robot model if (bounds.max_acceleration_ < 0.0) { - RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", + RCLCPP_ERROR(getLogger(), "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", bounds.max_acceleration_, vars[idx].c_str()); return false; } @@ -1076,10 +1082,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( } if (!set_acceleration_limit) { - RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint " - << vars[idx].c_str() - << "! You have to define acceleration limits in the URDF or " - "joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " + << vars[idx].c_str() + << "! You have to define acceleration limits in the URDF or " + "joint_limits.yaml"); return false; } } @@ -1099,7 +1105,7 @@ bool totgComputeTimeStamps(const size_t num_waypoints, robot_trajectory::RobotTr if (num_waypoints < 2) { - RCLCPP_ERROR(LOGGER, "computeTimeStamps() requires num_waypoints > 1"); + RCLCPP_ERROR(getLogger(), "computeTimeStamps() requires num_waypoints > 1"); return false; } @@ -1122,14 +1128,14 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { - RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for"); + RCLCPP_ERROR(getLogger(), "It looks like the planner did not set the group the plan was computed for"); return false; } if (hasMixedJointTypes(group)) { - RCLCPP_WARN(LOGGER, "There is a combination of revolute and prismatic joints in the robot model. TOTG's " - "`path_tolerance` will not function correctly."); + RCLCPP_WARN(getLogger(), "There is a combination of revolute and prismatic joints in the robot model. TOTG's " + "`path_tolerance` will not function correctly."); } const unsigned num_points = trajectory.getWayPointCount(); @@ -1183,7 +1189,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t Trajectory parameterized(Path(points, path_tolerance_), max_velocity, max_acceleration, DEFAULT_TIMESTEP); if (!parameterized.isValid()) { - RCLCPP_ERROR(LOGGER, "Unable to parameterize trajectory."); + RCLCPP_ERROR(getLogger(), "Unable to parameterize trajectory."); return false; } @@ -1251,8 +1257,8 @@ double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double request } else { - RCLCPP_WARN(LOGGER, "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", limit_type_str.c_str(), - requested_scaling_factor, scaling_factor); + RCLCPP_WARN(getLogger(), "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", + limit_type_str.c_str(), requested_scaling_factor, scaling_factor); } return scaling_factor; } diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 2c3703743f..7ed4f0debf 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -3,7 +3,7 @@ target_include_directories(moveit_transforms PUBLIC $ $ ) -target_link_libraries(moveit_transforms moveit_macros) +target_link_libraries(moveit_transforms moveit_macros moveit_utils) set_target_properties(moveit_transforms PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_transforms geometric_shapes diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index 446c847d0d..a18de07889 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -40,20 +40,27 @@ #include #include #include +#include namespace moveit { namespace core { -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_transforms.transforms"); +namespace +{ +rclcpp::Logger getLogger() +{ + static auto logger = moveit::makeChildLogger("transforms"); + return logger; +} +} // namespace Transforms::Transforms(const std::string& target_frame) : target_frame_(target_frame) { boost::trim(target_frame_); if (target_frame_.empty()) { - RCLCPP_ERROR(LOGGER, "The target frame for MoveIt Transforms cannot be empty."); + RCLCPP_ERROR(getLogger(), "The target frame for MoveIt Transforms cannot be empty."); } else { @@ -111,8 +118,8 @@ const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame) // If no transform found in map, return identity } - RCLCPP_ERROR(LOGGER, "Unable to transform from frame '%s' to frame '%s'. Returning identity.", from_frame.c_str(), - target_frame_.c_str()); + RCLCPP_ERROR(getLogger(), "Unable to transform from frame '%s' to frame '%s'. Returning identity.", + from_frame.c_str(), target_frame_.c_str()); // return identity static const Eigen::Isometry3d IDENTITY = Eigen::Isometry3d::Identity(); @@ -136,7 +143,7 @@ void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& fro ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry if (from_frame.empty()) { - RCLCPP_ERROR(LOGGER, "Cannot record transform with empty name"); + RCLCPP_ERROR(getLogger(), "Cannot record transform with empty name"); } else transforms_map_[from_frame] = t; @@ -158,7 +165,7 @@ void Transforms::setTransform(const geometry_msgs::msg::TransformStamped& transf } else { - RCLCPP_ERROR(LOGGER, "Given transform is to frame '%s', but frame '%s' was expected.", + RCLCPP_ERROR(getLogger(), "Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str()); } } diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt index d216427c20..f111592c7d 100644 --- a/moveit_core/utils/test/CMakeLists.txt +++ b/moveit_core/utils/test/CMakeLists.txt @@ -27,7 +27,9 @@ add_launch_test(rosout_publish_test.py # These tests do not work on Humble as /rosout logging from child loggers # does not work. -if(NOT $ENV{ROS_DISTRO} STREQUAL "humble") +# ROS_DISTRO is not defined on rolling +if((NOT DEFINED ENV{ROS_DISTRO}) OR + (NOT $ENV{ROS_DISTRO} STREQUAL "humble")) # Test init node logging then log from child logger to rosout add_launch_test(rosout_publish_test.py TARGET test-node_logging_from_child diff --git a/moveit_core/utils/test/logger_from_child_dut.cpp b/moveit_core/utils/test/logger_from_child_dut.cpp index f983e3b5eb..ef7b3de1e0 100644 --- a/moveit_core/utils/test/logger_from_child_dut.cpp +++ b/moveit_core/utils/test/logger_from_child_dut.cpp @@ -38,6 +38,17 @@ #include #include +// make the child logger the first time we use it +rclcpp::Logger getLogger() +{ + static auto logger = [] { + auto logger = moveit::makeChildLogger("child"); + RCLCPP_INFO(logger, "making the child logger"); + return logger; + }(); + return logger; +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -45,12 +56,10 @@ int main(int argc, char** argv) // Set the moveit logger to be from node moveit::setLogger(node->get_logger()); - - // Make a child logger - const auto child_logger = moveit::makeChildLogger("child"); + RCLCPP_INFO(moveit::getLogger(), "node logger"); // publish via a timer - auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), - [&] { RCLCPP_INFO(child_logger, "hello from child node!"); }); + auto wall_timer = + node->create_wall_timer(std::chrono::milliseconds(100), [&] { RCLCPP_INFO(getLogger(), "child node logger"); }); rclcpp::spin(node); } diff --git a/moveit_core/utils/test/logger_from_only_child_dut.cpp b/moveit_core/utils/test/logger_from_only_child_dut.cpp index c261e8cddf..3d8ec8a92c 100644 --- a/moveit_core/utils/test/logger_from_only_child_dut.cpp +++ b/moveit_core/utils/test/logger_from_only_child_dut.cpp @@ -38,16 +38,24 @@ #include #include +// make the child logger the first time we use it +rclcpp::Logger getLogger() +{ + static auto logger = [] { + auto logger = moveit::makeChildLogger("child"); + RCLCPP_INFO(logger, "making the child logger"); + return logger; + }(); + return logger; +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); - // Make a child logger - const auto child_logger = moveit::makeChildLogger("child"); - // publish via a timer auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), - [&] { RCLCPP_INFO(child_logger, "hello from only the child node!"); }); + [&] { RCLCPP_INFO(getLogger(), "hello from only the child node!"); }); rclcpp::spin(node); } From 2503535cde5cce24425d81ca4182cf5118e595b0 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 1 Nov 2023 15:46:57 -0600 Subject: [PATCH 2/8] prototype in logger_from_child_dut Signed-off-by: Tyler Weaver --- moveit_core/utils/src/logger.cpp | 1 + .../utils/test/logger_from_child_dut.cpp | 47 ++++++++++++------- 2 files changed, 32 insertions(+), 16 deletions(-) diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index f38bdd3bc7..5e545a9ee3 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -92,6 +92,7 @@ const rclcpp::Logger& getLogger() rclcpp::Logger makeChildLogger(const std::string& name) { + RCLCPP_INFO_STREAM(getLogger(), "Creating child logger: " << name); return getGlobalLoggerRef().get_child(name); } diff --git a/moveit_core/utils/test/logger_from_child_dut.cpp b/moveit_core/utils/test/logger_from_child_dut.cpp index ef7b3de1e0..7543e517e2 100644 --- a/moveit_core/utils/test/logger_from_child_dut.cpp +++ b/moveit_core/utils/test/logger_from_child_dut.cpp @@ -34,32 +34,47 @@ /* Author: Tyler Weaver */ -#include +#include +#include +#include +#include #include -#include +#include +#include -// make the child logger the first time we use it -rclcpp::Logger getLogger() +rclcpp::Logger& get_root_logger() { - static auto logger = [] { - auto logger = moveit::makeChildLogger("child"); - RCLCPP_INFO(logger, "making the child logger"); - return logger; - }(); + static auto moveit_node = std::make_shared(fmt::format("____moveit_{}", rsl::rng()())); + static auto logger = moveit_node->get_logger(); return logger; } +void set_node_logger_name(const std::string& name) +{ + static auto node = std::make_shared("moveit", name); + get_root_logger() = node->get_logger(); +} + +rclcpp::Logger get_logger(const std::string& name) +{ + return get_root_logger().get_child(name); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); + set_node_logger_name(node->get_name()); + + RCLCPP_ERROR(get_logger("child1"), "A"); + RCLCPP_ERROR(get_logger("child2"), "A"); + RCLCPP_ERROR(get_logger("child3"), "A"); + RCLCPP_ERROR(get_logger("child4"), "A"); - // Set the moveit logger to be from node - moveit::setLogger(node->get_logger()); - RCLCPP_INFO(moveit::getLogger(), "node logger"); + RCLCPP_ERROR(get_logger("child1"), "B"); + RCLCPP_ERROR(get_logger("child2"), "B"); + RCLCPP_ERROR(get_logger("child3"), "B"); + RCLCPP_ERROR(get_logger("child4"), "B"); - // publish via a timer - auto wall_timer = - node->create_wall_timer(std::chrono::milliseconds(100), [&] { RCLCPP_INFO(getLogger(), "child node logger"); }); - rclcpp::spin(node); + std::this_thread::sleep_for(std::chrono::seconds(1)); } From d83b147ae900b43a77d2faeae088fa40e813eaa4 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 2 Nov 2023 15:56:00 -0600 Subject: [PATCH 3/8] yet another refactor --- .../src/allvalid/collision_env_allvalid.cpp | 3 +- .../src/collision_common.cpp | 3 +- .../collision_detection/src/collision_env.cpp | 3 +- .../src/collision_matrix.cpp | 3 +- .../src/collision_octomap_filter.cpp | 3 +- .../src/collision_plugin_cache.cpp | 3 +- moveit_core/collision_detection/src/world.cpp | 3 +- .../bullet_integration/ros_bullet_utils.cpp | 4 +- .../src/collision_common.cpp | 3 +- .../src/collision_env_fcl.cpp | 3 +- .../collision_distance_field_types.h | 4 +- .../src/collision_common_distance_field.cpp | 3 +- .../src/collision_env_distance_field.cpp | 4 +- .../src/constraint_sampler.cpp | 3 +- .../src/constraint_sampler_manager.cpp | 3 +- .../src/constraint_sampler_tools.cpp | 3 +- .../src/default_constraint_samplers.cpp | 3 +- .../src/union_constraint_sampler.cpp | 3 +- .../distance_field/src/distance_field.cpp | 3 +- .../src/propagation_distance_field.cpp | 3 +- .../dynamics_solver/src/dynamics_solver.cpp | 3 +- moveit_core/exceptions/src/exceptions.cpp | 4 +- .../src/kinematic_constraint.cpp | 3 +- .../kinematic_constraints/src/utils.cpp | 3 +- .../moveit/kinematics_base/kinematics_base.h | 10 ++- .../kinematics_base/src/kinematics_base.cpp | 3 +- .../src/kinematics_metrics.cpp | 3 +- .../src/planning_interface.cpp | 3 +- .../planning_scene/src/planning_scene.cpp | 3 +- .../robot_model/src/floating_joint_model.cpp | 3 +- .../robot_model/src/joint_model_group.cpp | 3 +- moveit_core/robot_model/src/robot_model.cpp | 3 +- .../src/cartesian_interpolator.cpp | 3 +- moveit_core/robot_state/src/conversions.cpp | 3 +- moveit_core/robot_state/src/robot_state.cpp | 3 +- .../robot_trajectory/src/robot_trajectory.cpp | 3 +- .../test/test_robot_trajectory.cpp | 10 +-- .../src/ruckig_traj_smoothing.cpp | 3 +- .../time_optimal_trajectory_generation.cpp | 3 +- moveit_core/transforms/src/transforms.cpp | 3 +- .../utils/include/moveit/utils/logger.hpp | 34 +++----- moveit_core/utils/src/logger.cpp | 38 +++------ moveit_core/utils/test/CMakeLists.txt | 29 +------ moveit_core/utils/test/logger_dut.cpp | 4 +- .../utils/test/logger_from_child_dut.cpp | 80 ------------------- .../utils/test/logger_from_only_child_dut.cpp | 61 -------------- .../benchmarks/src/BenchmarkExecutor.cpp | 9 ++- .../benchmarks/src/BenchmarkOptions.cpp | 8 ++ moveit_ros/benchmarks/src/RunBenchmark.cpp | 16 ++-- ...pply_planning_scene_service_capability.cpp | 3 +- .../cartesian_path_service_capability.cpp | 5 ++ .../clear_octomap_service_capability.cpp | 14 +++- .../execute_trajectory_action_capability.cpp | 11 ++- .../kinematics_service_capability.cpp | 2 +- .../move_action_capability.cpp | 10 ++- .../plan_service_capability.cpp | 14 +++- .../tf_publisher_capability.cpp | 3 +- moveit_ros/move_group/src/move_group.cpp | 37 +++++---- .../move_group/src/move_group_capability.cpp | 61 ++++++++------ .../move_group/src/move_group_context.cpp | 30 ++++--- .../demos/cpp_interface/demo_joint_jog.cpp | 9 +-- .../demos/cpp_interface/demo_pose.cpp | 9 +-- .../demos/cpp_interface/demo_twist.cpp | 9 +-- .../moveit_servo/src/collision_monitor.cpp | 13 ++- moveit_ros/moveit_servo/src/servo.cpp | 2 +- moveit_ros/moveit_servo/src/servo_node.cpp | 22 +++-- moveit_ros/moveit_servo/src/utils/command.cpp | 6 +- .../src/occupancy_map_monitor.cpp | 2 +- ...ccupancy_map_monitor_middleware_handle.cpp | 4 +- .../src/occupancy_map_server.cpp | 11 ++- .../src/occupancy_map_updater.cpp | 11 ++- .../src/depth_image_octomap_updater.cpp | 2 +- .../src/lazy_free_space_updater.cpp | 2 +- .../mesh_filter/src/gl_renderer.cpp | 2 +- .../src/shape_mask.cpp | 17 ++-- .../src/pointcloud_octomap_updater.cpp | 2 +- .../semantic_world/src/semantic_world.cpp | 2 +- .../src/collision_plugin_loader.cpp | 2 +- .../src/constraint_sampler_manager_loader.cpp | 2 +- .../kinematics_plugin_loader.h | 2 +- .../planning/moveit_cpp/src/moveit_cpp.cpp | 2 +- .../moveit_cpp/src/planning_component.cpp | 2 +- .../plan_execution/src/plan_execution.cpp | 2 +- .../src/display_random_state.cpp | 4 +- .../src/evaluate_collision_checking_speed.cpp | 15 +++- .../src/print_planning_model_info.cpp | 2 +- .../src/print_planning_scene_info.cpp | 6 +- .../src/publish_scene_from_text.cpp | 6 +- .../src/visualize_robot_collision_volume.cpp | 2 +- .../src/planning_pipeline.cpp | 4 +- .../src/planning_pipeline_interfaces.cpp | 2 +- .../src/resolve_constraint_frames.cpp | 2 +- .../src/validate_workspace_bounds.cpp | 2 +- .../src/add_time_optimal_parameterization.cpp | 2 +- .../src/current_state_monitor.cpp | 2 +- .../src/planning_scene_monitor.cpp | 2 +- .../src/trajectory_monitor.cpp | 2 +- .../include/moveit/rdf_loader/rdf_loader.h | 2 - .../planning/rdf_loader/src/rdf_loader.cpp | 31 ++++--- .../src/robot_model_loader.cpp | 4 +- .../src/trajectory_execution_manager.cpp | 4 +- .../src/move_group_interface.cpp | 6 +- .../src/planning_scene_interface.cpp | 14 ++-- .../src/interaction_handler.cpp | 5 +- .../src/kinematic_options.cpp | 2 +- .../src/robot_interaction.cpp | 2 +- .../src/motion_planning_display.cpp | 2 +- .../src/motion_planning_frame.cpp | 2 +- .../src/background_processing.cpp | 8 +- .../src/planning_scene_display.cpp | 2 +- .../src/trajectory_visualization.cpp | 2 +- .../src/trajectory_display.cpp | 2 +- .../moveit/warehouse/warehouse_connector.h | 2 - moveit_ros/warehouse/src/broadcast.cpp | 15 ++-- .../warehouse/src/constraints_storage.cpp | 2 +- moveit_ros/warehouse/src/import_from_text.cpp | 9 ++- .../warehouse/src/initialize_demo_db.cpp | 14 ++-- .../warehouse/src/planning_scene_storage.cpp | 2 +- .../src/planning_scene_world_storage.cpp | 3 +- moveit_ros/warehouse/src/save_as_text.cpp | 20 ++--- .../warehouse/src/save_to_warehouse.cpp | 35 ++++---- moveit_ros/warehouse/src/state_storage.cpp | 2 +- .../src/trajectory_constraints_storage.cpp | 3 +- .../warehouse/src/warehouse_connector.cpp | 19 +++-- .../warehouse/src/warehouse_services.cpp | 24 +++--- 125 files changed, 463 insertions(+), 572 deletions(-) delete mode 100644 moveit_core/utils/test/logger_from_child_dut.cpp delete mode 100644 moveit_core/utils/test/logger_from_only_child_dut.cpp diff --git a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index 5853a74053..8c140ee3c9 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -46,8 +46,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_world_allvalid"); - return logger; + return moveit::getLogger("collision_detection.world_allvalid"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_common.cpp b/moveit_core/collision_detection/src/collision_common.cpp index 8fab70e28d..d869e61f31 100644 --- a/moveit_core/collision_detection/src/collision_common.cpp +++ b/moveit_core/collision_detection/src/collision_common.cpp @@ -46,8 +46,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_common"); - return logger; + return moveit::getLogger("collision_detection_common"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp index 4299e25167..338567527d 100644 --- a/moveit_core/collision_detection/src/collision_env.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -44,8 +44,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_env"); - return logger; + return moveit::getLogger("collision_detection_env"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index de4065a6b5..1d727f0bb5 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -47,8 +47,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_matrix"); - return logger; + return moveit::getLogger("collision_detection_matrix"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index b365ffbee8..327a9789a0 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -49,8 +49,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_octomap_filter"); - return logger; + return moveit::getLogger("collision_detection_octomap_filter"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_plugin_cache.cpp b/moveit_core/collision_detection/src/collision_plugin_cache.cpp index d652104357..cad03b98cf 100644 --- a/moveit_core/collision_detection/src/collision_plugin_cache.cpp +++ b/moveit_core/collision_detection/src/collision_plugin_cache.cpp @@ -45,8 +45,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_plugin_cache"); - return logger; + return moveit::getLogger("collision_detection_plugin_cache"); } } // namespace diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index fa4dba35bd..ddd154aefa 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -46,8 +46,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_world"); - return logger; + return moveit::getLogger("collision_detection_world"); } } // namespace diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp index 8cdc0c1dec..9f9e713430 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp @@ -115,8 +115,6 @@ Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose) rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_detection_bullet"); - return logger; + return moveit::getLogger("collision_detection_bullet"); } - } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index e4ea209d2e..f0ef932dc7 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -60,8 +60,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("moveit_collision_detection_fcl"); - return logger; + return moveit::getLogger("moveit_collision_detection_fcl"); } } // namespace diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 56e2715404..e5b01de84f 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -55,8 +55,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("moveit_collision_detection_fcl"); - return logger; + return moveit::getLogger("moveit_collision_detection_fcl"); } // Check whether this FCL version supports the requested computations diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 34fa321d0d..b0f4a2ccd7 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -373,7 +373,7 @@ class PosedBodySphereDecompositionVector public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PosedBodySphereDecompositionVector() : logger_(moveit::makeChildLogger("posed_body_sphere_decomposition_vector")) + PosedBodySphereDecompositionVector() : logger_(moveit::getLogger("posed_body_sphere_decomposition_vector")) { } @@ -447,7 +447,7 @@ class PosedBodyPointDecompositionVector public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PosedBodyPointDecompositionVector() : logger_(moveit::makeChildLogger("posed_body_point_decomposition_vector")) + PosedBodyPointDecompositionVector() : logger_(moveit::getLogger("posed_body_point_decomposition_vector")) { } diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp index 7e79deb2c7..3cba64aadb 100644 --- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp @@ -50,8 +50,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("collision_common_distance_field"); - return logger; + return moveit::getLogger("collision_common_distance_field"); } } // namespace diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 92bffb492c..c9bb341352 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -61,7 +61,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double /*padding*/, double /*scale*/) - : CollisionEnv(robot_model), logger_(moveit::makeChildLogger("collision_robot_distance_field")) + : CollisionEnv(robot_model), logger_(moveit::getLogger("collision_robot_distance_field")) { initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); @@ -80,7 +80,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) - : CollisionEnv(robot_model, world, padding, scale), logger_(moveit::makeChildLogger("collision_robot_distance_field")) + : CollisionEnv(robot_model, world, padding, scale), logger_(moveit::getLogger("collision_robot_distance_field")) { initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index 69b022aacb..654e47bd7f 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -45,8 +45,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("constraint_sampler"); - return logger; + return moveit::getLogger("constraint_sampler"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index c187505d3b..16d401b16c 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -48,8 +48,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("constraint_sampler_manager"); - return logger; + return moveit::getLogger("constraint_sampler_manager"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 0f3d4600ee..5cdba1adb4 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -49,8 +49,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("constraint_sampler_tools"); - return logger; + return moveit::getLogger("constraint_sampler_tools"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 38f83f2a8c..0935213ac9 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -47,8 +47,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("default_constraint_samplers"); - return logger; + return moveit::getLogger("default_constraint_samplers"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 1a98aa9fd3..47db822504 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -47,8 +47,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("union_constraint_sampler"); - return logger; + return moveit::getLogger("union_constraint_sampler"); } } // namespace diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 758d439097..c4a94708f7 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -51,8 +51,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("distance_field"); - return logger; + return moveit::getLogger("distance_field"); } } // namespace diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index 575f01a88f..fcaa85b3b1 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -49,8 +49,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("propagation_distance_field"); - return logger; + return moveit::getLogger("propagation_distance_field"); } } // namespace diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 0a5cb0a861..8806f82345 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -49,8 +49,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("dynamics_solver"); - return logger; + return moveit::getLogger("dynamics_solver"); } } // namespace diff --git a/moveit_core/exceptions/src/exceptions.cpp b/moveit_core/exceptions/src/exceptions.cpp index 33ef0f8508..ca0087bd9e 100644 --- a/moveit_core/exceptions/src/exceptions.cpp +++ b/moveit_core/exceptions/src/exceptions.cpp @@ -45,11 +45,11 @@ namespace moveit ConstructException::ConstructException(const std::string& what_arg) : std::runtime_error(what_arg) { - RCLCPP_ERROR(getLogger(), "Error during construction of object: %s\nException thrown.", what_arg.c_str()); + RCLCPP_ERROR(getLogger("exception"), "Error during construction of object: %s\nException thrown.", what_arg.c_str()); } Exception::Exception(const std::string& what_arg) : std::runtime_error(what_arg) { - RCLCPP_ERROR(getLogger(), "%s\nException thrown.", what_arg.c_str()); + RCLCPP_ERROR(getLogger("exception"), "%s\nException thrown.", what_arg.c_str()); } } // namespace moveit diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 58d5194ec6..13a4cc5865 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -60,8 +60,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("kinematic_constraints"); - return logger; + return moveit::getLogger("kinematic_constraints"); } } // namespace diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 18e4f14684..a776d08df6 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -57,8 +57,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("moveit_kinematic_constraints"); - return logger; + return moveit::getLogger("moveit_kinematic_constraints"); } } // namespace diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index b5cc2fd7df..c51cd4572e 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -322,7 +322,8 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase } // Otherwise throw error because this function should have been implemented - RCLCPP_ERROR(moveit::getLogger(), "This kinematic solver does not support searchPositionIK with multiple poses"); + RCLCPP_ERROR(moveit::getLogger("kinematics_base"), + "This kinematic solver does not support searchPositionIK with multiple poses"); return false; } @@ -362,7 +363,8 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options, context_state); } - RCLCPP_ERROR(moveit::getLogger(), "This kinematic solver does not support IK solution cost functions"); + RCLCPP_ERROR(moveit::getLogger("kinematics_base"), + "This kinematic solver does not support IK solution cost functions"); return false; } @@ -435,8 +437,8 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase { if (tip_frames_.size() > 1) { - RCLCPP_ERROR(moveit::getLogger(), "This kinematic solver has more than one tip frame, " - "do not call getTipFrame()"); + RCLCPP_ERROR(moveit::getLogger("kinematics_base"), "This kinematic solver has more than one tip frame, " + "do not call getTipFrame()"); } return tip_frames_[0]; diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index a5c1503325..7b333109b3 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -45,8 +45,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("kinematics_base"); - return logger; + return moveit::getLogger("kinematics_base"); } } // namespace diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index 188a6e067b..d0c4625918 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -49,8 +49,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("kinematics_metrics"); - return logger; + return moveit::getLogger("kinematics_metrics"); } } // namespace diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 367f9cb6fc..7e37b942b8 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -47,8 +47,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("planning_interface"); - return logger; + return moveit::getLogger("planning_interface"); } } // namespace diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 27fde1df57..d75e0707f2 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -59,8 +59,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("planning_scene"); - return logger; + return moveit::getLogger("planning_scene"); } } // namespace diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 565533cedd..94fb8d2f6e 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -54,8 +54,7 @@ constexpr size_t STATE_SPACE_DIMENSION = 7; rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("floating_joint_model"); - return logger; + return moveit::getLogger("floating_joint_model"); } } // namespace diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 6f611e963b..dc9bfb39a5 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -54,8 +54,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("joint_model_group"); - return logger; + return moveit::getLogger("joint_model_group"); } // check if a parent or ancestor of joint is included in this group diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 828fb8c10e..7c5536c117 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -54,8 +54,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("robot_model"); - return logger; + return moveit::getLogger("robot_model"); } } // namespace diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index e99af0424d..5a9f7fbe31 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -57,8 +57,7 @@ namespace rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("cartesian_interpolator"); - return logger; + return moveit::getLogger("cartesian_interpolator"); } std::optional hasRelativeJointSpaceJump(const std::vector& waypoints, diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 9d8ce35310..59b95ca207 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -56,8 +56,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("conversions"); - return logger; + return moveit::getLogger("conversions"); } bool jointStateToRobotStateImpl(const sensor_msgs::msg::JointState& joint_state, RobotState& state) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 206bf99e1d..8efcc939ed 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -59,8 +59,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("robot_state"); - return logger; + return moveit::getLogger("robot_state"); } } // namespace diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 1e73da14f4..ee072f9929 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -52,8 +52,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("robot_trajectory"); - return logger; + return moveit::getLogger("robot_trajectory"); } } // namespace diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index eeb791e8ab..e663924c34 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -339,19 +339,19 @@ class OneRobot : public testing::Test trajectory = std::make_shared(robot_model_, arm_jmg_name_); - EXPECT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match"; - EXPECT_TRUE(trajectory->empty()) << "Generated trajectory not empty"; + ASSERT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match"; + ASSERT_TRUE(trajectory->empty()) << "Generated trajectory not empty"; double duration_from_previous = 0.1; std::size_t waypoint_count = 5; for (std::size_t ix = 0; ix < waypoint_count; ++ix) trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous); // Quick check that getDuration is working correctly - EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) + ASSERT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) << "Generated trajectory duration incorrect"; - EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size()) + ASSERT_EQ(waypoint_count, trajectory->getWayPointDurations().size()) << "Generated trajectory has the wrong number of waypoints"; - EXPECT_EQ(waypoint_count, trajectory->size()); + ASSERT_EQ(waypoint_count, trajectory->size()); } protected: diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index 7b86373886..bfa88e450d 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -57,8 +57,7 @@ constexpr double OVERSHOOT_CHECK_PERIOD = 0.01; // sec rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("ruckig_traj_smoothing"); - return logger; + return moveit::getLogger("ruckig_traj_smoothing"); } } // namespace diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 662f76087f..5f7a66c894 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -56,8 +56,7 @@ constexpr double DEFAULT_SCALING_FACTOR = 1.0; rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("time_optimal_trajectory_generation"); - return logger; + return moveit::getLogger("time_optimal_trajectory_generation"); } } // namespace diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index a18de07889..c2a2345301 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -50,8 +50,7 @@ namespace { rclcpp::Logger getLogger() { - static auto logger = moveit::makeChildLogger("transforms"); - return logger; + return moveit::getLogger("transforms"); } } // namespace diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 1b0fe49255..13889b61ae 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -42,30 +42,14 @@ namespace moveit { -// Get reference to global logger. -// Use `setLogger` to set the logger to a node logger. -// This can be used in place in macro logger lines. For example: -// ```c++ -// RCLCPP_WARN(moveit::getLogger(), "Something went wrong"); -// ``` -const rclcpp::Logger& getLogger(); - -// Make a child logger. -// This should only be used after a node logger has been set through the -// `setLogger` function as it calls `get_child` on the result of `getLogger`. -// -// On Humble the resulting child logger does not log to /rosout. -// We had the option of logging to /rosout or keeping namespaces (on Humble) -// and we chose namespaces as /rosout was not working before this change. -// -// On Iron and Rolling, child loggers log to /rosout. -rclcpp::Logger makeChildLogger(const std::string& name); - -// Set the global logger. -// Use: -// ```c++ -// moveit::setLogger(node->get_logger()); -// ``` -void setLogger(const rclcpp::Logger& logger); +/// @brief Call once after creating a node to initialize logging namespaces. +/// @code{C++} +/// rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("move_group"); +/// moveit::setNodeLoggerName(node->get_name()); +/// @endcode +void setNodeLoggerName(const std::string& name); + +/// @brief Creates a namespaced logger. +rclcpp::Logger getLogger(const std::string& name); } // namespace moveit diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index 5e545a9ee3..33c5e26e60 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -45,29 +45,14 @@ namespace moveit // This is the function that stores the global logger used by moveit. // As it returns a reference to the static logger it can be changed through the -// `setLogger` function. -// -// You can get a const reference to the logger through the public function -// `getLogger`. -// -// As there is no way to know if `getLogger` is called before `setLogger` -// we try to initialize a node that will setup /rosout logging with this logger. -// If that fails (likely due to rclcpp::init not haven't been called) we set the -// global logger to one that is not associated with a node. When a logger not -// associated with a node is used the logs only go to the console and files, -// they do not go to /rosout. This is because publishing is done by a node. -// -// The node and logger created here is not intended to be used. But if it is, -// we append a random number to the name of the node name and logger to make it -// unique. This helps to prevent problems that arise from when multiple -// nodes use the same name. -rclcpp::Logger& getGlobalLoggerRef() +// `setNodeLoggerName` function. +rclcpp::Logger& getGlobalRootLogger() { static rclcpp::Logger logger = [&] { // A random number is appended to the name used for the node to make it unique. // This unique node and logger name is only used if a user does not set a logger - // through the `setLogger` method to their node's logger. - auto name = fmt::format("moveit_{}", rsl::rng()()); + // through the `setNodeLoggerName` method to their node's logger. + auto name = fmt::format("____moveit_{}", rsl::rng()()); try { static auto* moveit_node = new rclcpp::Node(name); @@ -85,20 +70,15 @@ rclcpp::Logger& getGlobalLoggerRef() return logger; } -const rclcpp::Logger& getLogger() +void setNodeLoggerName(const std::string& name) { - return getGlobalLoggerRef(); + static auto node = std::make_shared("moveit", name); + getGlobalRootLogger() = node->get_logger(); } -rclcpp::Logger makeChildLogger(const std::string& name) +rclcpp::Logger getLogger(const std::string& name) { - RCLCPP_INFO_STREAM(getLogger(), "Creating child logger: " << name); - return getGlobalLoggerRef().get_child(name); -} - -void setLogger(const rclcpp::Logger& logger) -{ - getGlobalLoggerRef() = logger; + return getGlobalRootLogger().get_child(name); } } // namespace moveit diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt index f111592c7d..4006b8b755 100644 --- a/moveit_core/utils/test/CMakeLists.txt +++ b/moveit_core/utils/test/CMakeLists.txt @@ -2,43 +2,22 @@ add_executable(logger_dut logger_dut.cpp) target_link_libraries(logger_dut rclcpp::rclcpp moveit_utils) -add_executable(logger_from_child_dut logger_from_child_dut.cpp) -target_link_libraries(logger_from_child_dut rclcpp::rclcpp moveit_utils) - -add_executable(logger_from_only_child_dut logger_from_only_child_dut.cpp) -target_link_libraries(logger_from_only_child_dut rclcpp::rclcpp moveit_utils) - -# Install is needed to run these as launch tests +# Install is needed to for launchtest to execute install( TARGETS logger_dut - logger_from_child_dut - logger_from_only_child_dut DESTINATION lib/${PROJECT_NAME} ) find_package(launch_testing_ament_cmake) -# Test node logger to rosout -add_launch_test(rosout_publish_test.py - TARGET test-node_logging - ARGS "dut:=logger_dut" -) - # These tests do not work on Humble as /rosout logging from child loggers # does not work. # ROS_DISTRO is not defined on rolling if((NOT DEFINED ENV{ROS_DISTRO}) OR (NOT $ENV{ROS_DISTRO} STREQUAL "humble")) - # Test init node logging then log from child logger to rosout - add_launch_test(rosout_publish_test.py - TARGET test-node_logging_from_child - ARGS "dut:=logger_from_child_dut" - ) - - # Test init only creating child logger and logging goes to rosout - add_launch_test(rosout_publish_test.py - TARGET test-logger_from_only_child_dut - ARGS "dut:=logger_from_only_child_dut" + add_launch_test(rosout_publish_test.py + TARGET test-node_logging + ARGS "dut:=logger_dut" ) endif() diff --git a/moveit_core/utils/test/logger_dut.cpp b/moveit_core/utils/test/logger_dut.cpp index 1a7147321a..59e2c29540 100644 --- a/moveit_core/utils/test/logger_dut.cpp +++ b/moveit_core/utils/test/logger_dut.cpp @@ -44,11 +44,11 @@ int main(int argc, char** argv) rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); // Set the moveit logger to be from node - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); // A node logger, should be in the file output and rosout auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), - [&] { RCLCPP_INFO(moveit::getLogger(), "hello from node!"); }); + [&] { RCLCPP_INFO(moveit::getLogger("child"), "hello from node!"); }); rclcpp::spin(node); } diff --git a/moveit_core/utils/test/logger_from_child_dut.cpp b/moveit_core/utils/test/logger_from_child_dut.cpp deleted file mode 100644 index 7543e517e2..0000000000 --- a/moveit_core/utils/test/logger_from_child_dut.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, PickNik Robotics Inc. - * 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: Tyler Weaver */ - -#include -#include -#include -#include -#include -#include -#include - -rclcpp::Logger& get_root_logger() -{ - static auto moveit_node = std::make_shared(fmt::format("____moveit_{}", rsl::rng()())); - static auto logger = moveit_node->get_logger(); - return logger; -} - -void set_node_logger_name(const std::string& name) -{ - static auto node = std::make_shared("moveit", name); - get_root_logger() = node->get_logger(); -} - -rclcpp::Logger get_logger(const std::string& name) -{ - return get_root_logger().get_child(name); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); - set_node_logger_name(node->get_name()); - - RCLCPP_ERROR(get_logger("child1"), "A"); - RCLCPP_ERROR(get_logger("child2"), "A"); - RCLCPP_ERROR(get_logger("child3"), "A"); - RCLCPP_ERROR(get_logger("child4"), "A"); - - RCLCPP_ERROR(get_logger("child1"), "B"); - RCLCPP_ERROR(get_logger("child2"), "B"); - RCLCPP_ERROR(get_logger("child3"), "B"); - RCLCPP_ERROR(get_logger("child4"), "B"); - - std::this_thread::sleep_for(std::chrono::seconds(1)); -} diff --git a/moveit_core/utils/test/logger_from_only_child_dut.cpp b/moveit_core/utils/test/logger_from_only_child_dut.cpp deleted file mode 100644 index 3d8ec8a92c..0000000000 --- a/moveit_core/utils/test/logger_from_only_child_dut.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, PickNik Robotics Inc. - * 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: Tyler Weaver */ - -#include -#include -#include - -// make the child logger the first time we use it -rclcpp::Logger getLogger() -{ - static auto logger = [] { - auto logger = moveit::makeChildLogger("child"); - RCLCPP_INFO(logger, "making the child logger"); - return logger; - }(); - return logger; -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); - - // publish via a timer - auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), - [&] { RCLCPP_INFO(getLogger(), "hello from only the child node!"); }); - rclcpp::spin(node); -} diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 5de41d7724..a646de69c0 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -62,9 +62,16 @@ #undef max -using moveit::getLogger; using namespace moveit_ros_benchmarks; +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("BenchmarkExecutor"); +} +} // namespace + template boost::posix_time::ptime toBoost(const std::chrono::time_point& from) { diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index d381905edd..b4f64e082d 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -40,6 +40,14 @@ using moveit::getLogger; using namespace moveit_ros_benchmarks; +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("BenchmarkOptions"); +} +} // namespace + BenchmarkOptions::BenchmarkOptions(const rclcpp::Node::SharedPtr& node) { if (!readBenchmarkOptions(node)) diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index ff68587b0f..e1a86af6d1 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -55,7 +55,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("moveit_run_benchmark", node_options); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); // Read benchmark options from param server moveit_ros_benchmarks::BenchmarkOptions options(node); @@ -66,7 +66,7 @@ int main(int argc, char** argv) options.getPlanningPipelineNames(planning_pipelines); if (!server.initialize(planning_pipelines)) { - RCLCPP_ERROR(getLogger(), "Failed to initialize benchmark server."); + RCLCPP_ERROR(node->get_logger(), "Failed to initialize benchmark server."); rclcpp::shutdown(); return 1; } @@ -83,18 +83,18 @@ int main(int argc, char** argv) { auto planning_scene_storage = moveit_warehouse::PlanningSceneStorage(warehouse_connection); planning_scene_storage.getPlanningSceneNames(scene_names); - RCLCPP_INFO(getLogger(), "Loaded scene names"); + RCLCPP_INFO(node->get_logger(), "Loaded scene names"); } else { - RCLCPP_ERROR(getLogger(), "Failed to load scene names from DB"); + RCLCPP_ERROR(node->get_logger(), "Failed to load scene names from DB"); rclcpp::shutdown(); return 1; } } catch (std::exception& e) { - RCLCPP_ERROR(getLogger(), "Failed to load scene names from DB: '%s'", e.what()); + RCLCPP_ERROR(node->get_logger(), "Failed to load scene names from DB: '%s'", e.what()); rclcpp::shutdown(); return 1; } @@ -104,7 +104,7 @@ int main(int argc, char** argv) options.scene_name = name; if (!server.runBenchmarks(options)) { - RCLCPP_ERROR(getLogger(), "Failed to run all benchmarks"); + RCLCPP_ERROR(node->get_logger(), "Failed to run all benchmarks"); } } } @@ -112,11 +112,11 @@ int main(int argc, char** argv) { if (!server.runBenchmarks(options)) { - RCLCPP_ERROR(getLogger(), "Failed to run all benchmarks"); + RCLCPP_ERROR(node->get_logger(), "Failed to run all benchmarks"); } } - RCLCPP_INFO(getLogger(), "Finished benchmarking"); + RCLCPP_INFO(node->get_logger(), "Finished benchmarking"); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp index 160ff597bd..df7089e305 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp @@ -67,7 +67,8 @@ bool ApplyPlanningSceneService::applyScene(const std::shared_ptrplanning_scene_monitor_) { - RCLCPP_ERROR(moveit::getLogger(), "Cannot apply PlanningScene as no scene is monitored."); + RCLCPP_ERROR(moveit::getLogger("ApplyPlanningSceneService"), + "Cannot apply PlanningScene as no scene is monitored."); return true; } context_->planning_scene_monitor_->updateFrameTransforms(); diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 148acbf8d3..8e2c696672 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -65,6 +65,11 @@ bool isStateValid(const planning_scene::PlanningScene* planning_scene, return (!planning_scene || !planning_scene->isStateColliding(*state, group->getName())) && (!constraint_set || constraint_set->decide(*state).satisfied); } + +rclcpp::Logger getLogger() +{ + return moveit::getLogger("cartesian_path_service_capability"); +} } // namespace namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp index e901dbdae7..af9231d74f 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp @@ -39,6 +39,14 @@ #include #include +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ClearOctomapService"); +} +} // namespace + move_group::ClearOctomapService::ClearOctomapService() : MoveGroupCapability("ClearOctomapService") { } @@ -55,11 +63,11 @@ void move_group::ClearOctomapService::clearOctomap(const std::shared_ptr& /*res*/) { if (!context_->planning_scene_monitor_) - RCLCPP_ERROR(moveit::getLogger(), "Cannot clear octomap since planning_scene_monitor_ does not exist."); + RCLCPP_ERROR(getLogger(), "Cannot clear octomap since planning_scene_monitor_ does not exist."); - RCLCPP_INFO(moveit::getLogger(), "Clearing octomap..."); + RCLCPP_INFO(getLogger(), "Clearing octomap..."); context_->planning_scene_monitor_->clearOctomap(); - RCLCPP_INFO(moveit::getLogger(), "Octomap cleared."); + RCLCPP_INFO(getLogger(), "Octomap cleared."); } #include diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index 3665844051..8cf823e23e 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -45,6 +45,13 @@ namespace move_group { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ClearOctomapService"); +} +} // namespace MoveGroupExecuteTrajectoryAction::MoveGroupExecuteTrajectoryAction() : MoveGroupCapability("ExecuteTrajectoryAction") { @@ -100,7 +107,7 @@ void MoveGroupExecuteTrajectoryAction::executePathCallback(const std::shared_ptr void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr& goal, std::shared_ptr& action_res) { - RCLCPP_INFO(moveit::getLogger(), "Execution request received"); + RCLCPP_INFO(getLogger(), "Execution request received"); context_->trajectory_execution_manager_->clear(); if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names)) @@ -124,7 +131,7 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptrerror_code.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED; } - RCLCPP_INFO_STREAM(moveit::getLogger(), "Execution completed: " << status.asString()); + RCLCPP_INFO_STREAM(getLogger(), "Execution completed: " << status.asString()); } else { diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index da3bf60515..3cd14be915 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -205,7 +205,7 @@ bool MoveGroupKinematicsService::computeFKService(const std::shared_ptrfk_link_names.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "No links specified for FK request"); + RCLCPP_ERROR(moveit::getLogger("MoveGroupKinematicsService"), "No links specified for FK request"); res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME; return true; } diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 7ab35acafa..34dba3fac0 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -45,11 +45,17 @@ #include #include -using moveit::getLogger; - namespace move_group { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("MoveGroupMoveAction"); +} +} // namespace + MoveGroupMoveAction::MoveGroupMoveAction() : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false } { diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp index 790a94c7a9..b6b59472c1 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp @@ -44,6 +44,14 @@ namespace move_group { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("MoveGroupPlanService"); +} +} // namespace + MoveGroupPlanService::MoveGroupPlanService() : MoveGroupCapability("MotionPlanService") { } @@ -62,7 +70,7 @@ bool MoveGroupPlanService::computePlanService(const std::shared_ptr& req, const std::shared_ptr& res) { - RCLCPP_INFO(moveit::getLogger(), "Received new planning service request..."); + RCLCPP_INFO(getLogger(), "Received new planning service request..."); // before we start planning, ensure that we have the latest robot state received... if (static_cast(req->motion_plan_request.start_state.is_diff)) context_->planning_scene_monitor_->waitForCurrentRobotState(context_->moveit_cpp_->getNode()->get_clock()->now()); @@ -83,14 +91,14 @@ bool MoveGroupPlanService::computePlanService(const std::shared_ptrgeneratePlan(ps, req->motion_plan_request, mp_res, context_->debug_)) { - RCLCPP_ERROR(moveit::getLogger(), "Generating a plan with planning pipeline failed."); + RCLCPP_ERROR(getLogger(), "Generating a plan with planning pipeline failed."); mp_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } mp_res.getMessage(res->motion_plan_response); } catch (std::exception& ex) { - RCLCPP_ERROR(moveit::getLogger(), "Planning pipeline threw an exception: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "Planning pipeline threw an exception: %s", ex.what()); res->motion_plan_response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index 9c31a80969..247749cef9 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -132,7 +132,8 @@ void TfPublisher::initialize() keep_running_ = true; - RCLCPP_INFO(moveit::getLogger(), "Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_); + RCLCPP_INFO(moveit::getLogger("TfPublisher"), "Initializing MoveGroupTfPublisher with a frame publishing rate of %d", + rate_); thread_ = std::thread(&TfPublisher::publishPlanningSceneFrames, this); } } // namespace move_group diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index e4dc16186d..0550c42860 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -51,6 +51,14 @@ static const std::string ROBOT_DESCRIPTION = namespace move_group { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("move_group"); +} +} // namespace + // These capabilities are loaded unless listed in disable_capabilities // clang-format off static const char* const DEFAULT_CAPABILITIES[] = { @@ -109,7 +117,7 @@ class MoveGroupExe } } else - RCLCPP_ERROR(moveit::getLogger(), "No MoveGroup context created. Nothing will work."); + RCLCPP_ERROR(getLogger(), "No MoveGroup context created. Nothing will work."); } MoveGroupContextPtr getContext() @@ -127,7 +135,7 @@ class MoveGroupExe } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL_STREAM(moveit::getLogger(), + RCLCPP_FATAL_STREAM(getLogger(), "Exception while creating plugin loader for move_group capabilities: " << ex.what()); return; } @@ -182,7 +190,7 @@ class MoveGroupExe } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR_STREAM(moveit::getLogger(), + RCLCPP_ERROR_STREAM(getLogger(), "Exception while loading move_group capability '" << capability << "': " << ex.what()); } } @@ -195,7 +203,7 @@ class MoveGroupExe for (const MoveGroupCapabilityPtr& cap : capabilities_) ss << "* - " << cap->getName() << '\n'; ss << "********************************************************" << '\n'; - RCLCPP_INFO(moveit::getLogger(), "%s", ss.str().c_str()); + RCLCPP_INFO(getLogger(), "%s", ss.str().c_str()); } MoveGroupContextPtr context_; @@ -212,7 +220,7 @@ int main(int argc, char** argv) opt.allow_undeclared_parameters(true); opt.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("move_group", opt); - moveit::setLogger(nh->get_logger()); + moveit::setNodeLoggerName(nh->get_name()); moveit_cpp::MoveItCpp::Options moveit_cpp_options(nh); // Prepare PlanningPipelineOptions @@ -222,7 +230,7 @@ int main(int argc, char** argv) { if (planning_pipeline_configs.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "Failed to read parameter 'move_group.planning_pipelines'"); + RCLCPP_ERROR(nh->get_logger(), "Failed to read parameter 'move_group.planning_pipelines'"); } else { @@ -241,7 +249,7 @@ int main(int argc, char** argv) // Ignore default_planning_pipeline if there is no matching entry in pipeline_names if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end()) { - RCLCPP_WARN(moveit::getLogger(), + RCLCPP_WARN(nh->get_logger(), "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines", default_planning_pipeline.c_str()); default_planning_pipeline = ""; // reset invalid pipeline id @@ -250,7 +258,7 @@ int main(int argc, char** argv) else if (pipeline_names.size() > 1) // only warn if there are multiple pipelines to choose from { // Handle deprecated move_group.launch - RCLCPP_WARN(moveit::getLogger(), + RCLCPP_WARN(nh->get_logger(), "MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default " "planning pipeline configuration"); } @@ -260,13 +268,12 @@ int main(int argc, char** argv) { if (!pipeline_names.empty()) { - RCLCPP_WARN(moveit::getLogger(), "Using default pipeline '%s'", pipeline_names[0].c_str()); + RCLCPP_WARN(nh->get_logger(), "Using default pipeline '%s'", pipeline_names[0].c_str()); default_planning_pipeline = pipeline_names[0]; } else { - RCLCPP_WARN(moveit::getLogger(), - "Falling back to using the the move_group node namespace (deprecated behavior)."); + RCLCPP_WARN(nh->get_logger(), "Falling back to using the the move_group node namespace (deprecated behavior)."); default_planning_pipeline = "move_group"; moveit_cpp_options.planning_pipeline_options.pipeline_names = { default_planning_pipeline }; moveit_cpp_options.planning_pipeline_options.parent_namespace = nh->get_effective_namespace(); @@ -294,11 +301,11 @@ int main(int argc, char** argv) debug = true; if (debug) { - RCLCPP_INFO(moveit::getLogger(), "MoveGroup debug mode is ON"); + RCLCPP_INFO(nh->get_logger(), "MoveGroup debug mode is ON"); } else { - RCLCPP_INFO(moveit::getLogger(), "MoveGroup debug mode is OFF"); + RCLCPP_INFO(nh->get_logger(), "MoveGroup debug mode is OFF"); } rclcpp::executors::MultiThreadedExecutor executor; @@ -308,7 +315,7 @@ int main(int argc, char** argv) bool monitor_dynamics; if (nh->get_parameter("monitor_dynamics", monitor_dynamics) && monitor_dynamics) { - RCLCPP_INFO(moveit::getLogger(), "MoveGroup monitors robot dynamics (higher load)"); + RCLCPP_INFO(nh->get_logger(), "MoveGroup monitors robot dynamics (higher load)"); planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); } @@ -321,7 +328,7 @@ int main(int argc, char** argv) rclcpp::shutdown(); } else - RCLCPP_ERROR(moveit::getLogger(), "Planning scene not configured"); + RCLCPP_ERROR(nh->get_logger(), "Planning scene not configured"); return 0; } diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index ddfb0965f0..66be57ddfd 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -44,14 +44,24 @@ #include #include -void move_group::MoveGroupCapability::setContext(const MoveGroupContextPtr& context) +namespace move_group +{ +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("MoveGroupCapability"); +} +} // namespace + +void MoveGroupCapability::setContext(const MoveGroupContextPtr& context) { context_ = context; } -void move_group::MoveGroupCapability::convertToMsg(const std::vector& trajectory, - moveit_msgs::msg::RobotState& first_state_msg, - std::vector& trajectory_msg) const +void MoveGroupCapability::convertToMsg(const std::vector& trajectory, + moveit_msgs::msg::RobotState& first_state_msg, + std::vector& trajectory_msg) const { if (!trajectory.empty()) { @@ -72,9 +82,9 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vectorempty()) { @@ -83,13 +93,13 @@ void move_group::MoveGroupCapability::convertToMsg(const robot_trajectory::Robot } } -void move_group::MoveGroupCapability::convertToMsg(const std::vector& trajectory, - moveit_msgs::msg::RobotState& first_state_msg, - moveit_msgs::msg::RobotTrajectory& trajectory_msg) const +void MoveGroupCapability::convertToMsg(const std::vector& trajectory, + moveit_msgs::msg::RobotState& first_state_msg, + moveit_msgs::msg::RobotTrajectory& trajectory_msg) const { if (trajectory.size() > 1) { - RCLCPP_ERROR_STREAM(moveit::getLogger(), + RCLCPP_ERROR_STREAM(getLogger(), "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!"); } if (!trajectory.empty()) @@ -99,31 +109,31 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vectorplanning_scene_monitor_->getTFClient()) return false; @@ -204,14 +214,13 @@ bool move_group::MoveGroupCapability::performTransform(geometry_msgs::msg::PoseS } catch (tf2::TransformException& ex) { - RCLCPP_ERROR(moveit::getLogger(), "TF Problem: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "TF Problem: %s", ex.what()); return false; } return true; } -planning_pipeline::PlanningPipelinePtr -move_group::MoveGroupCapability::resolvePlanningPipeline(const std::string& pipeline_id) const +planning_pipeline::PlanningPipelinePtr MoveGroupCapability::resolvePlanningPipeline(const std::string& pipeline_id) const { if (pipeline_id.empty()) { @@ -224,14 +233,16 @@ move_group::MoveGroupCapability::resolvePlanningPipeline(const std::string& pipe try { auto pipeline = context_->moveit_cpp_->getPlanningPipelines().at(pipeline_id); - RCLCPP_INFO(moveit::getLogger(), "Using planning pipeline '%s'", pipeline_id.c_str()); + RCLCPP_INFO(getLogger(), "Using planning pipeline '%s'", pipeline_id.c_str()); return pipeline; } catch (const std::out_of_range&) { - RCLCPP_WARN(moveit::getLogger(), "Couldn't find requested planning pipeline '%s'", pipeline_id.c_str()); + RCLCPP_WARN(getLogger(), "Couldn't find requested planning pipeline '%s'", pipeline_id.c_str()); } } return planning_pipeline::PlanningPipelinePtr(); } + +} // namespace move_group diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index 616df10bee..c7cabc81d9 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -41,9 +41,19 @@ #include #include -move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& moveit_cpp, - const std::string& default_planning_pipeline, - bool allow_trajectory_execution, bool debug) +namespace move_group +{ +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("MoveGroupContext"); +} +} // namespace + +MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& moveit_cpp, + const std::string& default_planning_pipeline, bool allow_trajectory_execution, + bool debug) : moveit_cpp_(moveit_cpp) , planning_scene_monitor_(moveit_cpp->getPlanningSceneMonitorNonConst()) , allow_trajectory_execution_(allow_trajectory_execution) @@ -59,7 +69,7 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m else { RCLCPP_ERROR( - moveit::getLogger(), + getLogger(), "Failed to find default PlanningPipeline '%s' - please check MoveGroup's planning pipeline configuration.", default_planning_pipeline.c_str()); } @@ -72,7 +82,7 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m } } -move_group::MoveGroupContext::~MoveGroupContext() +MoveGroupContext::~MoveGroupContext() { plan_execution_.reset(); trajectory_execution_manager_.reset(); @@ -80,20 +90,22 @@ move_group::MoveGroupContext::~MoveGroupContext() planning_scene_monitor_.reset(); } -bool move_group::MoveGroupContext::status() const +bool MoveGroupContext::status() const { const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager(); if (planner_interface) { - RCLCPP_INFO_STREAM(moveit::getLogger(), + RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); - RCLCPP_INFO_STREAM(moveit::getLogger(), "MoveGroup context initialization complete"); + RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context initialization complete"); return true; } else { - RCLCPP_WARN_STREAM(moveit::getLogger(), + RCLCPP_WARN_STREAM(getLogger(), "MoveGroup running was unable to load " << planning_pipeline_->getPlannerPluginName()); return false; } } + +} // namespace move_group diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp index 3636b23059..1f978d2ed9 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp @@ -45,7 +45,6 @@ #include #include -using moveit::getLogger; using namespace moveit_servo; int main(int argc, char* argv[]) @@ -54,7 +53,7 @@ int main(int argc, char* argv[]) // The servo object expects to get a ROS node. const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); - moveit::setLogger(demo_node->get_logger()); + moveit::setNodeLoggerName(demo_node->get_name()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -88,7 +87,7 @@ int main(int argc, char* argv[]) std::chrono::seconds time_elapsed(0); auto start_time = std::chrono::steady_clock::now(); - RCLCPP_INFO_STREAM(getLogger(), servo.getStatusMessage()); + RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage()); while (rclcpp::ok()) { const KinematicState joint_state = servo.getNextJointState(joint_jog); @@ -98,7 +97,7 @@ int main(int argc, char* argv[]) time_elapsed = std::chrono::duration_cast(current_time - start_time); if (time_elapsed > timeout_duration) { - RCLCPP_INFO_STREAM(getLogger(), "Timed out"); + RCLCPP_INFO_STREAM(demo_node->get_logger(), "Timed out"); break; } else if (status != StatusCode::INVALID) @@ -108,6 +107,6 @@ int main(int argc, char* argv[]) rate.sleep(); } - RCLCPP_INFO(getLogger(), "Exiting demo."); + RCLCPP_INFO(demo_node->get_logger(), "Exiting demo."); rclcpp::shutdown(); } diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index f1426f6e7c..d2ac48d746 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -48,7 +48,6 @@ #include #include -using moveit::getLogger; using namespace moveit_servo; namespace @@ -63,7 +62,7 @@ int main(int argc, char* argv[]) // The servo object expects to get a ROS node. const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); - moveit::setLogger(demo_node->get_logger()); + moveit::setNodeLoggerName(demo_node->get_name()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -135,7 +134,7 @@ int main(int argc, char* argv[]) // Frequency at which commands will be sent to the robot controller. rclcpp::WallRate command_rate(50); - RCLCPP_INFO_STREAM(getLogger(), servo.getStatusMessage()); + RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage()); while (!stop_tracking && rclcpp::ok()) { @@ -157,12 +156,12 @@ int main(int argc, char* argv[]) command_rate.sleep(); } - RCLCPP_INFO_STREAM(getLogger(), "REACHED : " << stop_tracking); + RCLCPP_INFO_STREAM(demo_node->get_logger(), "REACHED : " << stop_tracking); stop_tracking = true; if (tracker_thread.joinable()) tracker_thread.join(); - RCLCPP_INFO(getLogger(), "Exiting demo."); + RCLCPP_INFO(demo_node->get_logger(), "Exiting demo."); rclcpp::shutdown(); } diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp index b3b6512bfc..1ffa3f8696 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -47,7 +47,6 @@ #include #include -using moveit::getLogger; using namespace moveit_servo; int main(int argc, char* argv[]) @@ -56,7 +55,7 @@ int main(int argc, char* argv[]) // The servo object expects to get a ROS node. const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); - moveit::setLogger(demo_node->get_logger()); + moveit::setNodeLoggerName(demo_node->get_name()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -92,7 +91,7 @@ int main(int argc, char* argv[]) std::chrono::seconds time_elapsed(0); auto start_time = std::chrono::steady_clock::now(); - RCLCPP_INFO_STREAM(getLogger(), servo.getStatusMessage()); + RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage()); while (rclcpp::ok()) { const KinematicState joint_state = servo.getNextJointState(target_twist); @@ -102,7 +101,7 @@ int main(int argc, char* argv[]) time_elapsed = std::chrono::duration_cast(current_time - start_time); if (time_elapsed > timeout_duration) { - RCLCPP_INFO_STREAM(getLogger(), "Timed out"); + RCLCPP_INFO_STREAM(demo_node->get_logger(), "Timed out"); break; } else if (status != StatusCode::INVALID) @@ -112,6 +111,6 @@ int main(int argc, char* argv[]) rate.sleep(); } - RCLCPP_INFO(getLogger(), "Exiting demo."); + RCLCPP_INFO(demo_node->get_logger(), "Exiting demo."); rclcpp::shutdown(); } diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp index 5a84a98a37..23cc60216f 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -43,6 +43,13 @@ namespace moveit_servo { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("CollisionMonitor"); +} +} // namespace CollisionMonitor::CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const servo::Params& servo_params, std::atomic& collision_velocity_scale) @@ -63,11 +70,11 @@ void CollisionMonitor::start() if (!monitor_thread_.joinable()) { monitor_thread_ = std::thread(&CollisionMonitor::checkCollisions, this); - RCLCPP_INFO_STREAM(moveit::getLogger(), "Collision monitor started"); + RCLCPP_INFO_STREAM(getLogger(), "Collision monitor started"); } else { - RCLCPP_ERROR_STREAM(moveit::getLogger(), "Collision monitor could not be started"); + RCLCPP_ERROR_STREAM(getLogger(), "Collision monitor could not be started"); } } @@ -78,7 +85,7 @@ void CollisionMonitor::stop() { monitor_thread_.join(); } - RCLCPP_INFO_STREAM(moveit::getLogger(), "Collision monitor stopped"); + RCLCPP_INFO_STREAM(getLogger(), "Collision monitor stopped"); } void CollisionMonitor::checkCollisions() diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 5583974717..40229ebf28 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -58,7 +58,7 @@ namespace moveit_servo Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : node_(node) - , logger_(moveit::makeChildLogger("servo")) + , logger_(moveit::getLogger("servo")) , servo_param_listener_{ std::move(servo_param_listener) } , planning_scene_monitor_{ planning_scene_monitor } { diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 5c5c872b9e..0d1eada3a8 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -42,8 +42,6 @@ #include #include -using moveit::getLogger; - namespace moveit_servo { @@ -67,11 +65,11 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) , new_twist_msg_{ false } , new_pose_msg_{ false } { - moveit::setLogger(node_->get_logger()); + moveit::setNodeLoggerName(node_->get_name()); if (!options.use_intra_process_comms()) { - RCLCPP_WARN_STREAM(getLogger(), + RCLCPP_WARN_STREAM(node_->get_logger(), "Intra-process communication is disabled, consider enabling it by adding: " "\nextra_arguments=[{'use_intra_process_comms' : True}]\nto the Servo composable node " "in the launch file"); @@ -82,16 +80,16 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) { if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority)) { - RCLCPP_INFO_STREAM(getLogger(), "Realtime kernel available, higher thread priority has been set."); + RCLCPP_INFO_STREAM(node_->get_logger(), "Realtime kernel available, higher thread priority has been set."); } else { - RCLCPP_WARN_STREAM(getLogger(), "Could not enable FIFO RT scheduling policy."); + RCLCPP_WARN_STREAM(node_->get_logger(), "Could not enable FIFO RT scheduling policy."); } } else { - RCLCPP_WARN_STREAM(getLogger(), "Realtime kernel is recommended for better performance."); + RCLCPP_WARN_STREAM(node_->get_logger(), "Realtime kernel is recommended for better performance."); } std::shared_ptr servo_param_listener = @@ -182,7 +180,7 @@ void ServoNode::switchCommandType(const std::shared_ptrcommand_type << " requested"); + RCLCPP_WARN_STREAM(node_->get_logger(), "Unknown command type " << request->command_type << " requested"); } response->success = (request->command_type == static_cast(servo_->getCommandType())); } @@ -225,7 +223,7 @@ std::optional ServoNode::processJointJogCommand() if (new_joint_jog_msg_) { next_joint_state = result.second; - RCLCPP_DEBUG_STREAM(getLogger(), "Joint jog command timed out. Halting to a stop."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Joint jog command timed out. Halting to a stop."); } } @@ -257,7 +255,7 @@ std::optional ServoNode::processTwistCommand() if (new_twist_msg_) { next_joint_state = result.second; - RCLCPP_DEBUG_STREAM(getLogger(), "Twist command timed out. Halting to a stop."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Twist command timed out. Halting to a stop."); } } @@ -286,7 +284,7 @@ std::optional ServoNode::processPoseCommand() if (new_pose_msg_) { next_joint_state = result.second; - RCLCPP_DEBUG_STREAM(getLogger(), "Pose command timed out. Halting to a stop."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Pose command timed out. Halting to a stop."); } } @@ -326,7 +324,7 @@ void ServoNode::servoLoop() else if (new_joint_jog_msg_ || new_twist_msg_ || new_pose_msg_) { new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false; - RCLCPP_WARN_STREAM(getLogger(), "Command type has not been set, cannot accept input"); + RCLCPP_WARN_STREAM(node_->get_logger(), "Command type has not been set, cannot accept input"); } if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID) && diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index fe8f038ee7..f88a64a64e 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -40,10 +40,12 @@ #include #include -using moveit::getLogger; - namespace { +rclcpp::Logger getLogger() +{ + return moveit::getLogger("servo_command"); +} /** * @brief Helper function to create a move group deltas vector from a sub group deltas vector. A delta vector for the diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index 3f04e106eb..5153362fe5 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -73,7 +73,7 @@ OccupancyMapMonitor::OccupancyMapMonitor(std::unique_ptr middl , debug_info_{ false } , mesh_handle_count_{ 0 } , active_{ false } - , logger_(moveit::makeChildLogger("occupancy_map_monitor")) + , logger_(moveit::getLogger("occupancy_map_monitor")) { if (middleware_handle_ == nullptr) { diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp index 8fcfdd7a5f..92d39d9f1c 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp @@ -53,9 +53,7 @@ namespace occupancy_map_monitor OccupancyMapMonitorMiddlewareHandle::OccupancyMapMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node, double map_resolution, const std::string& map_frame) - : node_{ node } - , parameters_{ map_resolution, map_frame, {} } - , logger_(moveit::makeChildLogger("occupancy_map_monitor")) + : node_{ node }, parameters_{ map_resolution, map_frame, {} }, logger_(moveit::getLogger("occupancy_map_monitor")) { try { diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index 03672ac04a..c0c095f493 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -58,7 +58,7 @@ #include static void publishOctomap(const rclcpp::Publisher::SharedPtr& octree_binary_pub, - occupancy_map_monitor::OccupancyMapMonitor& server) + occupancy_map_monitor::OccupancyMapMonitor& server, rclcpp::Logger logger) { octomap_msgs::msg::Octomap map; @@ -73,7 +73,7 @@ static void publishOctomap(const rclcpp::Publisher:: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(moveit::getLogger(), steady_clock, 1000, "Could not generate OctoMap message"); + RCLCPP_ERROR_THROTTLE(logger, steady_clock, 1000, "Could not generate OctoMap message"); #pragma GCC diagnostic pop } } @@ -81,7 +81,7 @@ static void publishOctomap(const rclcpp::Publisher:: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(moveit::getLogger(), steady_clock, 1000, "Exception thrown while generating OctoMap message"); + RCLCPP_ERROR_THROTTLE(logger, steady_clock, 1000, "Exception thrown while generating OctoMap message"); #pragma GCC diagnostic pop } server.getOcTreePtr()->unlockRead(); @@ -93,13 +93,16 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("occupancy_map_server"); + moveit::setNodeLoggerName(node->get_name()); auto octree_binary_pub = node->create_publisher("octomap_binary", RMW_QOS_POLICY_HISTORY_KEEP_LAST); auto clock_ptr = std::make_shared(RCL_ROS_TIME); std::shared_ptr buffer = std::make_shared(clock_ptr, tf2::durationFromSec(5.0)); tf2_ros::TransformListener listener(*buffer, node, false /* spin_thread - disables executor */); occupancy_map_monitor::OccupancyMapMonitor server(node, buffer); - server.setUpdateCallback([&octree_binary_pub, &server] { return publishOctomap(octree_binary_pub, server); }); + server.setUpdateCallback([&octree_binary_pub, &server, logger = node->get_logger()] { + return publishOctomap(octree_binary_pub, server, logger); + }); server.startMonitor(); rclcpp::spin(node); diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp index 3e255cbac5..d16c79291a 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp @@ -44,6 +44,13 @@ namespace occupancy_map_monitor { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("OccupancyMapUpdater"); +} +} // namespace OccupancyMapUpdater::OccupancyMapUpdater(const std::string& type) : type_(type) { @@ -89,7 +96,7 @@ bool OccupancyMapUpdater::updateTransformCache(const std::string& target_frame, #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" RCLCPP_ERROR_THROTTLE( - moveit::getLogger(), steady_clock, 1000, + getLogger(), steady_clock, 1000, "Transform cache was not updated. Self-filtering may fail. If transforms were not available yet, consider " "setting robot_description_planning.shape_transform_cache_lookup_wait_time to wait longer for transforms"); #pragma GCC diagnostic pop @@ -101,7 +108,7 @@ bool OccupancyMapUpdater::updateTransformCache(const std::string& target_frame, rclcpp::Clock steady_clock(RCL_STEADY_TIME); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_THROTTLE(moveit::getLogger(), steady_clock, 1000, + RCLCPP_WARN_THROTTLE(getLogger(), steady_clock, 1000, "No callback provided for updating the transform cache for octomap updaters"); #pragma GCC diagnostic pop return false; diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 54b1e8aedf..7f06e53c90 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -71,7 +71,7 @@ DepthImageOctomapUpdater::DepthImageOctomapUpdater() , K2_(0.0) , K4_(0.0) , K5_(0.0) - , logger_(moveit::makeChildLogger("depth_image_octomap_updater")) + , logger_(moveit::getLogger("depth_image_octomap_updater")) { } diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index 32b7669474..2486738ad6 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -51,7 +51,7 @@ LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const collision_detection::OccMapTree , process_model_cells_set_(nullptr) , update_thread_([this] { lazyUpdateThread(); }) , process_thread_([this] { processThread(); }) - , logger_(moveit::makeChildLogger("lazy_free_space_updater")) + , logger_(moveit::getLogger("lazy_free_space_updater")) { } diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 0f7a694e54..b8f03d45fc 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -344,7 +344,7 @@ GLuint mesh_filter::GLRenderer::loadShaders(const string& vertex_source, const s glGetProgramInfoLog(program_id, info_log_length, nullptr, &program_error_message[0]); std::size_t l = strnlen(&program_error_message[0], program_error_message.size()); if (l > 0) - RCLCPP_ERROR(moveit::getLogger(), "%s\n", &program_error_message[0]); + RCLCPP_ERROR(moveit::getLogger("gl_renderer"), "%s\n", &program_error_message[0]); } if (vertex_shader_id) diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index cff2da74c2..fde28345d2 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -41,6 +41,14 @@ #include #include +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("shape_mask"); +} +} // namespace + point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) : transform_callback_(transform_callback), next_handle_(1), min_handle_(1) { @@ -81,8 +89,7 @@ point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addSh std::pair::iterator, bool> insert_op = bodies_.insert(ss); if (!insert_op.second) { - RCLCPP_ERROR(moveit::getLogger(), - "Internal error in management of bodies in ShapeMask. This is a serious error."); + RCLCPP_ERROR(getLogger(), "Internal error in management of bodies in ShapeMask. This is a serious error."); } used_handles_[next_handle_] = insert_op.first; } @@ -116,7 +123,7 @@ void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle) min_handle_ = handle; } else - RCLCPP_ERROR(moveit::getLogger(), "Unable to remove shape handle %u", handle); + RCLCPP_ERROR(getLogger(), "Unable to remove shape handle %u", handle); } void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::msg::PointCloud2& data_in, @@ -143,12 +150,12 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::msg { if (!it->body) { - RCLCPP_ERROR_STREAM(moveit::getLogger(), + RCLCPP_ERROR_STREAM(getLogger(), "Missing transform for shape with handle " << it->handle << " without a body"); } else { - RCLCPP_ERROR_STREAM(moveit::getLogger(), + RCLCPP_ERROR_STREAM(getLogger(), "Missing transform for shape " << it->body->getType() << " with handle " << it->handle); } } diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index bd7fa2b99b..315bb5d607 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -58,7 +58,7 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater() , max_update_rate_(0) , point_cloud_subscriber_(nullptr) , point_cloud_filter_(nullptr) - , logger_(moveit::makeChildLogger("pointcloud_octomap_updater")) + , logger_(moveit::getLogger("pointcloud_octomap_updater")) { } diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 9eba619e43..4ed23cb54e 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -67,7 +67,7 @@ namespace semantic_world SemanticWorld::SemanticWorld(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningSceneConstPtr& planning_scene) - : planning_scene_(planning_scene), node_handle_(node), logger_(moveit::makeChildLogger("semantic_world")) + : planning_scene_(planning_scene), node_handle_(node), logger_(moveit::getLogger("semantic_world")) { table_subscriber_ = node_handle_->create_subscription( diff --git a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp index af7b7a5e43..89a5d7fb07 100644 --- a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp +++ b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp @@ -39,7 +39,7 @@ namespace collision_detection { static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_plugin_loader"); -CollisionPluginLoader::CollisionPluginLoader() : logger_(moveit::makeChildLogger("collision_plugin_loader")) +CollisionPluginLoader::CollisionPluginLoader() : logger_(moveit::getLogger("collision_plugin_loader")) { } diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp index 77904474ff..2d92f3195d 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp +++ b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp @@ -50,7 +50,7 @@ class ConstraintSamplerManagerLoader::Helper { public: Helper(const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) - : node_(node), logger_(moveit::makeChildLogger("constraint_sampler_manager_loader")) + : node_(node), logger_(moveit::getLogger("constraint_sampler_manager_loader")) { if (node_->has_parameter("constraint_samplers")) { diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index 756e11825d..4adf24ee4f 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -57,7 +57,7 @@ class KinematicsPluginLoader well as used to read the SRDF document when needed. */ KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node, const std::string& robot_description = "robot_description") - : node_(node), robot_description_(robot_description), logger_(moveit::makeChildLogger("kinematics_plugin_loader")) + : node_(node), robot_description_(robot_description), logger_(moveit::getLogger("kinematics_plugin_loader")) { } diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index d3c0bb967d..9f12282120 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -50,7 +50,7 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node) : MoveItCpp(node, Opti } MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options) - : node_(node), logger_(moveit::makeChildLogger("moveit_cpp")) + : node_(node), logger_(moveit::getLogger("moveit_cpp")) { // Configure planning scene monitor if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options)) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index c3877e39f9..270b9357cc 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -51,7 +51,7 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt : node_(moveit_cpp->getNode()) , moveit_cpp_(moveit_cpp) , group_name_(group_name) - , logger_(moveit::makeChildLogger("planning_component")) + , logger_(moveit::getLogger("planning_component")) { joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); if (!joint_model_group_) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 5e0133e6ce..76cb40c836 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -83,7 +83,7 @@ plan_execution::PlanExecution::PlanExecution( : node_(node) , planning_scene_monitor_(planning_scene_monitor) , trajectory_execution_manager_(trajectory_execution) - , logger_(moveit::makeChildLogger("add_time_optimal_parameterization")) + , logger_(moveit::getLogger("add_time_optimal_parameterization")) { if (!trajectory_execution_manager_) { diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index 0f8a91f828..52b79968d4 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -46,7 +46,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("display_random_state"); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); bool valid = false; bool invalid = false; @@ -81,7 +81,7 @@ int main(int argc, char** argv) { if (!psm.getPlanningScene()) { - RCLCPP_ERROR(moveit::getLogger(), "Planning scene did not load properly, exiting..."); + RCLCPP_ERROR(node->get_logger(), "Planning scene did not load properly, exiting..."); break; } diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index 7f497fd10b..8c94f8433f 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -41,9 +41,16 @@ #include #include -using moveit::getLogger; using namespace std::chrono_literals; +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("evaluate_collision_checking_speed"); +} +} // namespace + static const std::string ROBOT_DESCRIPTION = "robot_description"; void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene& scene, @@ -67,7 +74,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("evaluate_collision_checking_speed"); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); unsigned int nthreads = 2; unsigned int trials = 10000; @@ -106,7 +113,7 @@ int main(int argc, char** argv) rclcpp::sleep_for(500ms); std::vector states; - RCLCPP_INFO(getLogger(), "Sampling %u valid states...", nthreads); + RCLCPP_INFO(node->get_logger(), "Sampling %u valid states...", nthreads); for (unsigned int i = 0; i < nthreads; ++i) { // sample a valid state @@ -140,7 +147,7 @@ int main(int argc, char** argv) } } else - RCLCPP_ERROR(getLogger(), "Planning scene not configured"); + RCLCPP_ERROR(node->get_logger(), "Planning scene not configured"); return 0; } diff --git a/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp b/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp index b12531c569..f868d5f622 100644 --- a/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp +++ b/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("print_model_info_to_console"); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp b/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp index 9e8a59a8cb..e8a8a8ec79 100644 --- a/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp +++ b/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp @@ -52,18 +52,18 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("print_scene_info_to_console"); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); - RCLCPP_INFO_STREAM(moveit::getLogger(), "Getting planning scene info to print"); + RCLCPP_INFO_STREAM(node->get_logger(), "Getting planning scene info to print"); // Create planning scene monitor planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR_STREAM(moveit::getLogger(), "Planning scene not configured"); + RCLCPP_ERROR_STREAM(node->get_logger(), "Planning scene not configured"); return 1; } diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index fb447ba239..d083fa46f6 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -56,7 +56,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("publish_planning_scene"); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); // decide whether to publish the full scene bool full_scene = false; @@ -101,7 +101,7 @@ int main(int argc, char** argv) std::ifstream f(argv[filename_index]); if (ps.loadGeometryFromStream(f)) { - RCLCPP_INFO(moveit::getLogger(), "Publishing geometry from '%s' ...", argv[filename_index]); + RCLCPP_INFO(node->get_logger(), "Publishing geometry from '%s' ...", argv[filename_index]); moveit_msgs::msg::PlanningScene ps_msg; ps.getPlanningSceneMsg(ps_msg); ps_msg.is_diff = true; @@ -124,7 +124,7 @@ int main(int argc, char** argv) } else { - RCLCPP_WARN(moveit::getLogger(), + RCLCPP_WARN(node->get_logger(), "A filename was expected as argument. That file should be a text representation of the geometry in a " "planning scene."); } diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index 9e0ce6fb7b..8b5f517535 100644 --- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp +++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp @@ -56,7 +56,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("visualize_robot_collision_volume"); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index e17d075489..87bd34b979 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -46,7 +46,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model , node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(model) - , logger_(moveit::makeChildLogger("planning_pipeline")) + , logger_(moveit::getLogger("planning_pipeline")) { auto param_listener = planning_pipeline_parameters::ParamListener(node, parameter_namespace); pipeline_parameters_ = param_listener.get_params(); @@ -63,7 +63,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model , node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(model) - , logger_(moveit::makeChildLogger("planning_pipeline")) + , logger_(moveit::getLogger("planning_pipeline")) { pipeline_parameters_.planning_plugin = planner_plugin_name; pipeline_parameters_.request_adapters = request_adapter_plugin_names; diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index ad9f8da382..b4158fb054 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -46,7 +46,7 @@ namespace planning_pipeline_interfaces rclcpp::Logger get_logger() { - static rclcpp::Logger logger = moveit::makeChildLogger("planning_pipeline_interfaces"); + static rclcpp::Logger logger = moveit::getLogger("planning_pipeline_interfaces"); return logger; } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index 1374c86c3b..d7a5d3e886 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -49,7 +49,7 @@ namespace default_planning_request_adapters class ResolveConstraintFrames : public planning_interface::PlanningRequestAdapter { public: - ResolveConstraintFrames() : logger_(moveit::makeChildLogger("resolve_constraint_frames")) + ResolveConstraintFrames() : logger_(moveit::getLogger("resolve_constraint_frames")) { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp index ce74da5456..5fcff9504a 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp @@ -53,7 +53,7 @@ namespace default_planning_request_adapters class ValidateWorkspaceBounds : public planning_interface::PlanningRequestAdapter { public: - ValidateWorkspaceBounds() : logger_(moveit::makeChildLogger("validate_workspace_bounds")) + ValidateWorkspaceBounds() : logger_(moveit::getLogger("validate_workspace_bounds")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp index 072b1c610b..9901bc08d3 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -49,7 +49,7 @@ using namespace trajectory_processing; class AddTimeOptimalParameterization : public planning_interface::PlanningResponseAdapter { public: - AddTimeOptimalParameterization() : logger_(moveit::makeChildLogger("add_time_optimal_parameterization")) + AddTimeOptimalParameterization() : logger_(moveit::getLogger("add_time_optimal_parameterization")) { } diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 50985e568e..bf7efdc410 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -60,7 +60,7 @@ CurrentStateMonitor::CurrentStateMonitor(std::unique_ptr::epsilon()) , use_sim_time_(use_sim_time) - , logger_(moveit::makeChildLogger("current_state_monitor")) + , logger_(moveit::getLogger("current_state_monitor")) { robot_state_.setToDefaultValues(); } diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 4921316479..ac0340a449 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -96,7 +96,7 @@ PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, , dt_state_update_(0.0) , shape_transform_cache_lookup_wait_time_(0, 0) , rm_loader_(rm_loader) - , logger_(moveit::makeChildLogger("planning_scene_monitor")) + , logger_(moveit::getLogger("planning_scene_monitor")) { std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp index 66e575b282..34095d4a23 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -55,7 +55,7 @@ planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor( , middleware_handle_(std::move(middleware_handle)) , sampling_frequency_(sampling_frequency) , trajectory_(current_state_monitor_->getRobotModel(), "") - , logger_(moveit::makeChildLogger("trajectory_monitor")) + , logger_(moveit::getLogger("trajectory_monitor")) { setSamplingFrequency(sampling_frequency); } diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index dae7523039..85d1fa28aa 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -133,7 +133,5 @@ class RDFLoader srdf::ModelSharedPtr srdf_; urdf::ModelInterfaceSharedPtr urdf_; - - rclcpp::Logger logger_; }; } // namespace rdf_loader diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index 88f37c435c..7ed8b3b273 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -56,10 +56,17 @@ namespace rdf_loader { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("RDFLoader"); +} +} // namespace RDFLoader::RDFLoader(const std::shared_ptr& node, const std::string& ros_name, bool default_continuous_value, double default_timeout) - : ros_name_(ros_name), logger_(moveit::makeChildLogger("rdf_loader")) + : ros_name_(ros_name) { auto start = node->now(); @@ -77,11 +84,11 @@ RDFLoader::RDFLoader(const std::shared_ptr& node, const std::strin return; } - RCLCPP_INFO_STREAM(logger_, "Loaded robot model in " << (node->now() - start).seconds() << " seconds"); + RCLCPP_INFO_STREAM(getLogger(), "Loaded robot model in " << (node->now() - start).seconds() << " seconds"); } RDFLoader::RDFLoader(const std::string& urdf_string, const std::string& srdf_string) - : urdf_string_(urdf_string), srdf_string_(srdf_string), logger_(moveit::makeChildLogger("rdf_loader")) + : urdf_string_(urdf_string), srdf_string_(srdf_string) { if (!loadFromStrings()) { @@ -94,14 +101,14 @@ bool RDFLoader::loadFromStrings() std::unique_ptr urdf = std::make_unique(); if (!urdf->initString(urdf_string_)) { - RCLCPP_INFO(logger_, "Unable to parse URDF"); + RCLCPP_INFO(getLogger(), "Unable to parse URDF"); return false; } srdf::ModelSharedPtr srdf = std::make_shared(); if (!srdf->initString(*urdf, srdf_string_)) { - RCLCPP_ERROR(logger_, "Unable to parse SRDF"); + RCLCPP_ERROR(getLogger(), "Unable to parse SRDF"); return false; } @@ -122,20 +129,20 @@ bool RDFLoader::loadFileToString(std::string& buffer, const std::string& path) { if (path.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "Path is empty"); + RCLCPP_ERROR(getLogger(), "Path is empty"); return false; } if (!std::filesystem::exists(path)) { - RCLCPP_ERROR(moveit::getLogger(), "File does not exist"); + RCLCPP_ERROR(getLogger(), "File does not exist"); return false; } std::ifstream stream(path.c_str()); if (!stream.good()) { - RCLCPP_ERROR(moveit::getLogger(), "Unable to load path"); + RCLCPP_ERROR(getLogger(), "Unable to load path"); return false; } @@ -155,13 +162,13 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa buffer.clear(); if (path.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "Path is empty"); + RCLCPP_ERROR(getLogger(), "Path is empty"); return false; } if (!std::filesystem::exists(path)) { - RCLCPP_ERROR(moveit::getLogger(), "File does not exist"); + RCLCPP_ERROR(getLogger(), "File does not exist"); return false; } @@ -177,7 +184,7 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa #endif if (!pipe) { - RCLCPP_ERROR(moveit::getLogger(), "Unable to load path"); + RCLCPP_ERROR(getLogger(), "Unable to load path"); return false; } @@ -217,7 +224,7 @@ bool RDFLoader::loadPkgFileToString(std::string& buffer, const std::string& pack } catch (const ament_index_cpp::PackageNotFoundError& e) { - RCLCPP_ERROR(moveit::getLogger(), "ament_index_cpp: %s", e.what()); + RCLCPP_ERROR(getLogger(), "ament_index_cpp: %s", e.what()); return false; } diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 7c03a1b49b..91dd24b22a 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -49,7 +49,7 @@ namespace robot_model_loader RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, bool load_kinematics_solvers) - : node_(node), logger_(moveit::makeChildLogger("robot_model_loader")) + : node_(node), logger_(moveit::getLogger("robot_model_loader")) { Options opt(robot_description); opt.load_kinematics_solvers = load_kinematics_solvers; @@ -57,7 +57,7 @@ RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const st } RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const Options& opt) - : node_(node), logger_(moveit::makeChildLogger("robot_model_loader")) + : node_(node), logger_(moveit::getLogger("robot_model_loader")) { configure(opt); } diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 148cf58e1c..886f9faccc 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -55,7 +55,7 @@ static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING = TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm) - : node_(node), logger_(moveit::makeChildLogger("trajectory_execution_manager")), robot_model_(robot_model), csm_(csm) + : node_(node), logger_(moveit::getLogger("trajectory_execution_manager")), robot_model_(robot_model), csm_(csm) { if (!node_->get_parameter("moveit_manage_controllers", manage_controllers_)) manage_controllers_ = false; @@ -67,7 +67,7 @@ TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::Share const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers) : node_(node) - , logger_(moveit::makeChildLogger("trajectory_execution_manager")) + , logger_(moveit::getLogger("trajectory_execution_manager")) , robot_model_(robot_model) , csm_(csm) , manage_controllers_(manage_controllers) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 21c3d1477e..25aa2c9c5b 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -110,7 +110,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl public: MoveGroupInterfaceImpl(const rclcpp::Node::SharedPtr& node, const Options& opt, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) - : opt_(opt), node_(node), logger_(moveit::makeChildLogger("move_group_interface")), tf_buffer_(tf_buffer) + : opt_(opt), node_(node), logger_(moveit::getLogger("move_group_interface")), tf_buffer_(tf_buffer) { // We have no control on how the passed node is getting executed. To make sure MGI is functional, we're creating // our own callback group which is managed in a separate callback thread @@ -1319,7 +1319,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group_name, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) - : logger_(moveit::makeChildLogger("move_group_interface")) + : logger_(moveit::getLogger("move_group_interface")) { if (!rclcpp::ok()) throw std::runtime_error("ROS does not seem to be running"); @@ -1330,7 +1330,7 @@ MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, cons MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) - : logger_(moveit::makeChildLogger("move_group_interface")) + : logger_(moveit::getLogger("move_group_interface")) { impl_ = new MoveGroupInterfaceImpl(node, opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); } diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index f71c2ab475..2e589cf982 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -43,8 +43,6 @@ #include #include -using moveit::getLogger; - namespace moveit { namespace planning_interface @@ -60,7 +58,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl "__node:=" + std::string("planning_scene_interface_") + std::to_string(reinterpret_cast(this)) }); node_ = rclcpp::Node::make_shared("_", ns, options); - moveit::setLogger(node_->get_logger()); + moveit::setNodeLoggerName(node_->get_name()); planning_scene_diff_publisher_ = node_->create_publisher("planning_scene", 1); planning_scene_service_ = node_->create_client(move_group::GET_PLANNING_SCENE_SERVICE_NAME); @@ -115,7 +113,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl auto res = planning_scene_service_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_WARN(getLogger(), "Could not call planning scene service to get object names"); + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object names"); return result; } response = res.get(); @@ -188,7 +186,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl auto res = planning_scene_service_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_WARN(getLogger(), "Could not call planning scene service to get object geometries"); + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object geometries"); return result; } response = res.get(); @@ -214,7 +212,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl auto res = planning_scene_service_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_WARN(getLogger(), "Could not call planning scene service to get attached object geometries"); + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get attached object geometries"); return result; } response = res.get(); @@ -240,7 +238,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl auto res = apply_planning_scene_service_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_WARN(getLogger(), "Failed to call ApplyPlanningScene service"); + RCLCPP_WARN(node_->get_logger(), "Failed to call ApplyPlanningScene service"); } response = res.get(); return response->success; @@ -292,7 +290,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl srv->wait_for_service(std::chrono::duration_cast(d)); if (!srv->service_is_ready()) { - RCLCPP_WARN_STREAM(getLogger(), + RCLCPP_WARN_STREAM(node_->get_logger(), "service '" << srv->get_service_name() << "' not advertised yet. Continue waiting..."); srv->wait_for_service(); } diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index 1a598e7e99..e1b41782f6 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -406,14 +406,15 @@ bool InteractionHandler::transformFeedbackPose( } catch (tf2::TransformException& e) { - RCLCPP_ERROR(moveit::getLogger(), "Error transforming from frame '%s' to frame '%s'", + RCLCPP_ERROR(moveit::getLogger("InteractionHandler"), "Error transforming from frame '%s' to frame '%s'", tpose.header.frame_id.c_str(), planning_frame_.c_str()); return false; } } else { - RCLCPP_ERROR(moveit::getLogger(), "Cannot transform from frame '%s' to frame '%s' (no TF instance provided)", + RCLCPP_ERROR(moveit::getLogger("InteractionHandler"), + "Cannot transform from frame '%s' to frame '%s' (no TF instance provided)", tpose.header.frame_id.c_str(), planning_frame_.c_str()); return false; } diff --git a/moveit_ros/robot_interaction/src/kinematic_options.cpp b/moveit_ros/robot_interaction/src/kinematic_options.cpp index df96d4f703..0e13c3c725 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -51,7 +51,7 @@ bool robot_interaction::KinematicOptions::setStateFromIK(moveit::core::RobotStat const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group); if (!jmg) { - RCLCPP_ERROR(moveit::getLogger(), "No getJointModelGroup('%s') found", group.c_str()); + RCLCPP_ERROR(moveit::getLogger("KinematicOptions"), "No getJointModelGroup('%s') found", group.c_str()); return false; } bool result = state.setFromIK(jmg, pose, tip, diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 9d431f192f..75ed047820 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -65,7 +65,7 @@ RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot const rclcpp::Node::SharedPtr& node, const std::string& ns) : robot_model_(robot_model) , node_(node) - , logger_(moveit::makeChildLogger("moveit_ros_robot_interaction")) + , logger_(moveit::getLogger("moveit_ros_robot_interaction")) , kinematic_options_map_(std::make_shared()) { topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index c1f03e7105..cd9fdfd205 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1037,7 +1037,7 @@ void MotionPlanningDisplay::changePlanningGroup(const std::string& group) planning_group_property_->setStdString(group); } else - RCLCPP_ERROR(moveit::getLogger(), "Group [%s] not found in the robot model.", group.c_str()); + RCLCPP_ERROR(moveit::getLogger("MotionPlanningDisplay"), "Group [%s] not found in the robot model.", group.c_str()); } void MotionPlanningDisplay::changedPlanningGroup() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index fe793b807e..229dcb9bc6 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -68,7 +68,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c , planning_display_(pdisplay) , context_(context) , ui_(new Ui::MotionPlanningUI()) - , logger_(moveit::makeChildLogger("motion_planning_frame")) + , logger_(moveit::getLogger("motion_planning_frame")) , first_time_(true) { auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp index 373be8c824..4104e8c707 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp @@ -79,14 +79,14 @@ void BackgroundProcessing::processingThread() action_lock_.unlock(); try { - RCLCPP_DEBUG(moveit::getLogger(), "Begin executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(moveit::getLogger("BackgroundProcessing"), "Begin executing '%s'", action_name.c_str()); fn(); - RCLCPP_DEBUG(moveit::getLogger(), "Done executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(moveit::getLogger("BackgroundProcessing"), "Done executing '%s'", action_name.c_str()); } catch (std::exception& ex) { - RCLCPP_ERROR(moveit::getLogger(), "Exception caught while processing action '%s': %s", action_name.c_str(), - ex.what()); + RCLCPP_ERROR(moveit::getLogger("BackgroundProcessing"), "Exception caught while processing action '%s': %s", + action_name.c_str(), ex.what()); } processing_ = false; if (queue_change_event_) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index c10985b5fb..16abd1515b 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -67,7 +67,7 @@ PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool s : Display() , planning_scene_needs_render_(true) , current_scene_time_(0.0f) - , logger_(moveit::makeChildLogger("planning_scene_display")) + , logger_(moveit::getLogger("planning_scene_display")) { move_group_ns_property_ = new rviz_common::properties::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in " diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index ba72717ddb..5ce7b776ae 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -72,7 +72,7 @@ TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Proper , widget_(widget) , trajectory_slider_panel_(nullptr) , trajectory_slider_dock_panel_(nullptr) - , logger_(moveit::makeChildLogger("trajectory_visualization")) + , logger_(moveit::getLogger("trajectory_visualization")) { trajectory_topic_property_ = new rviz_common::properties::RosTopicProperty( "Trajectory Topic", "/display_planned_path", diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index c028ccad31..36dc26edaa 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -45,7 +45,7 @@ namespace moveit_rviz_plugin { -TrajectoryDisplay::TrajectoryDisplay() : Display(), logger_(moveit::makeChildLogger("trajectory_display")) +TrajectoryDisplay::TrajectoryDisplay() : Display(), logger_(moveit::getLogger("trajectory_display")) { // The robot description property is only needed when using the trajectory playback standalone (not within motion diff --git a/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h index 86b8e18997..04e279a3ca 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h @@ -37,7 +37,6 @@ #pragma once #include -#include namespace moveit_warehouse { @@ -53,6 +52,5 @@ class WarehouseConnector private: std::string dbexec_; int child_pid_; - rclcpp::Logger logger_; }; } // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index d2b35bf596..7e17f6db14 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/src/broadcast.cpp @@ -66,7 +66,6 @@ static const std::string CONSTRAINTS_TOPIC = "constraints"; static const std::string STATES_TOPIC = "robot_states"; using namespace std::chrono_literals; -using moveit::getLogger; int main(int argc, char** argv) { @@ -75,7 +74,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("publish_warehouse_data", node_options); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); // time to wait in between publishing messages double delay = 0.001; @@ -142,7 +141,7 @@ int main(int argc, char** argv) moveit_warehouse::PlanningSceneWithMetadata pswm; if (pss.getPlanningScene(pswm, scene_name)) { - RCLCPP_INFO(getLogger(), "Publishing scene '%s'", + RCLCPP_INFO(node->get_logger(), "Publishing scene '%s'", pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str()); pub_scene->publish(static_cast(*pswm)); executor.spin_once(0ns); @@ -155,14 +154,14 @@ int main(int argc, char** argv) std::vector planning_queries; std::vector query_names; pss.getPlanningQueries(planning_queries, query_names, pswm->name); - RCLCPP_INFO(getLogger(), "There are %d planning queries associated to the scene", + RCLCPP_INFO(node->get_logger(), "There are %d planning queries associated to the scene", static_cast(planning_queries.size())); rclcpp::sleep_for(500ms); for (std::size_t i = 0; i < planning_queries.size(); ++i) { if (req) { - RCLCPP_INFO(getLogger(), "Publishing query '%s'", query_names[i].c_str()); + RCLCPP_INFO(node->get_logger(), "Publishing query '%s'", query_names[i].c_str()); pub_req->publish(static_cast(*planning_queries[i])); executor.spin_once(0ns); } @@ -196,7 +195,7 @@ int main(int argc, char** argv) moveit_warehouse::ConstraintsWithMetadata cwm; if (cs.getConstraints(cwm, cname)) { - RCLCPP_INFO(getLogger(), "Publishing constraints '%s'", + RCLCPP_INFO(node->get_logger(), "Publishing constraints '%s'", cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str()); pub_constr->publish(static_cast(*cwm)); executor.spin_once(0ns); @@ -218,7 +217,7 @@ int main(int argc, char** argv) moveit_warehouse::RobotStateWithMetadata rswm; if (rs.getRobotState(rswm, rname)) { - RCLCPP_INFO(getLogger(), "Publishing state '%s'", + RCLCPP_INFO(node->get_logger(), "Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str()); pub_state->publish(static_cast(*rswm)); executor.spin_once(0ns); @@ -228,7 +227,7 @@ int main(int argc, char** argv) } rclcpp::sleep_for(1s); - RCLCPP_INFO(getLogger(), "Done."); + RCLCPP_INFO(node->get_logger(), "Done."); return 0; } diff --git a/moveit_ros/warehouse/src/constraints_storage.cpp b/moveit_ros/warehouse/src/constraints_storage.cpp index ca56e58d6e..0fc4e7342e 100644 --- a/moveit_ros/warehouse/src/constraints_storage.cpp +++ b/moveit_ros/warehouse/src/constraints_storage.cpp @@ -49,7 +49,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::ConstraintsStorage::ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::makeChildLogger("moveit_warehouse_constraints_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_constraints_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index d8efdf869b..fe2aec4223 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -49,7 +49,10 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; -using moveit::getLogger; +rclcpp::Logger getLogger() +{ + return moveit::getLogger("import_from_text"); +} void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* psm, moveit_warehouse::RobotStateStorage* rs) @@ -223,7 +226,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("import_from_text_to_warehouse", node_options); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); // clang-format off boost::program_options::options_description desc; @@ -253,7 +256,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(getLogger(), "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(node->get_logger(), "Unable to initialize PlanningSceneMonitor"); return 1; } diff --git a/moveit_ros/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/src/initialize_demo_db.cpp index 23d92f6bc7..855714dcab 100644 --- a/moveit_ros/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/src/initialize_demo_db.cpp @@ -56,8 +56,6 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; -using moveit::getLogger; - int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -65,7 +63,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("initialize_demo_db", node_options); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -92,7 +90,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(getLogger(), "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(node->get_logger(), "Unable to initialize PlanningSceneMonitor"); return 1; } @@ -108,12 +106,12 @@ int main(int argc, char** argv) psm.getPlanningScene()->getPlanningSceneMsg(psmsg); psmsg.name = "default"; pss.addPlanningScene(psmsg); - RCLCPP_INFO(getLogger(), "Added default scene: '%s'", psmsg.name.c_str()); + RCLCPP_INFO(node->get_logger(), "Added default scene: '%s'", psmsg.name.c_str()); moveit_msgs::msg::RobotState rsmsg; moveit::core::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg); rs.addRobotState(rsmsg, "default"); - RCLCPP_INFO(getLogger(), "Added default state"); + RCLCPP_INFO(node->get_logger(), "Added default state"); const std::vector& gnames = psm.getRobotModel()->getJointModelGroupNames(); for (const std::string& gname : gnames) @@ -140,9 +138,9 @@ int main(int argc, char** argv) cmsg.orientation_constraints.resize(1, ocm); cmsg.name = ocm.link_name + ":upright"; cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName()); - RCLCPP_INFO(getLogger(), "Added default constraint: '%s'", cmsg.name.c_str()); + RCLCPP_INFO(node->get_logger(), "Added default constraint: '%s'", cmsg.name.c_str()); } - RCLCPP_INFO(getLogger(), "Default MoveIt Warehouse was reset."); + RCLCPP_INFO(node->get_logger(), "Default MoveIt Warehouse was reset."); rclcpp::spin(node); diff --git a/moveit_ros/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/src/planning_scene_storage.cpp index dd5bbd1996..b85749a850 100644 --- a/moveit_ros/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp @@ -49,7 +49,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::PlanningSceneStorage::PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::makeChildLogger("moveit_warehouse_planning_scene_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_planning_scene_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp index 066044b135..524bea290d 100644 --- a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp @@ -46,8 +46,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::PlanningSceneWorldStorage::PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)) - , logger_(moveit::makeChildLogger("moveit_warehouse_planning_scene_world_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_planning_scene_world_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index 7a1893c983..ae69f03ff6 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -56,9 +56,8 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; typedef std::pair LinkConstraintPair; typedef std::map LinkConstraintMap; -using moveit::getLogger; - -void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap) +void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap, + rclcpp::Logger logger) { for (const moveit_msgs::msg::PositionConstraint& position_constraint : constraints.position_constraints) { @@ -76,7 +75,7 @@ void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, Li } else { - RCLCPP_WARN(getLogger(), "Orientation constraint for %s has no matching position constraint", + RCLCPP_WARN(logger, "Orientation constraint for %s has no matching position constraint", orientation_constraint.link_name.c_str()); } } @@ -89,7 +88,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("save_warehouse_as_text", node_options); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -127,7 +126,7 @@ int main(int argc, char** argv) moveit_warehouse::PlanningSceneWithMetadata pswm; if (pss.getPlanningScene(pswm, scene_name)) { - RCLCPP_INFO(getLogger(), "Saving scene '%s'", scene_name.c_str()); + RCLCPP_INFO(node->get_logger(), "Saving scene '%s'", scene_name.c_str()); psm.getPlanningScene()->setPlanningSceneMsg(static_cast(*pswm)); std::ofstream fout((scene_name + ".scene").c_str()); psm.getPlanningScene()->saveGeometryToStream(fout); @@ -157,7 +156,8 @@ int main(int argc, char** argv) qfout << robot_state_names.size() << '\n'; for (const std::string& robot_state_name : robot_state_names) { - RCLCPP_INFO(getLogger(), "Saving start state %s for scene %s", robot_state_name.c_str(), scene_name.c_str()); + RCLCPP_INFO(node->get_logger(), "Saving start state %s for scene %s", robot_state_name.c_str(), + scene_name.c_str()); qfout << robot_state_name << '\n'; moveit_warehouse::RobotStateWithMetadata robot_state; rss.getRobotState(robot_state, robot_state_name); @@ -174,14 +174,14 @@ int main(int argc, char** argv) qfout << constraint_names.size() << '\n'; for (const std::string& constraint_name : constraint_names) { - RCLCPP_INFO(getLogger(), "Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str()); + RCLCPP_INFO(node->get_logger(), "Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str()); qfout << "link_constraint" << '\n'; qfout << constraint_name << '\n'; moveit_warehouse::ConstraintsWithMetadata constraints; cs.getConstraints(constraints, constraint_name); LinkConstraintMap lcmap; - collectLinkConstraints(*constraints, lcmap); + collectLinkConstraints(*constraints, lcmap, node->get_logger()); for (std::pair& iter : lcmap) { std::string link_name = iter.first; @@ -200,7 +200,7 @@ int main(int argc, char** argv) } } - RCLCPP_INFO(getLogger(), "Done."); + RCLCPP_INFO(node->get_logger(), "Done."); rclcpp::spin(node); return 0; } diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index 2dbcc20fc0..8d78327224 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -60,7 +60,10 @@ #include #include -using moveit::getLogger; +rclcpp::Logger getLogger() +{ + return moveit::getLogger("save_to_warehouse"); +} static const std::string ROBOT_DESCRIPTION = "robot_description"; @@ -138,7 +141,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("save_to_warehouse", node_options); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -165,7 +168,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(getLogger(), "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(node->get_logger(), "Unable to initialize PlanningSceneMonitor"); return 1; } @@ -178,35 +181,35 @@ int main(int argc, char** argv) pss.getPlanningSceneNames(names); if (names.empty()) { - RCLCPP_INFO(getLogger(), "There are no previously stored scenes"); + RCLCPP_INFO(node->get_logger(), "There are no previously stored scenes"); } else { - RCLCPP_INFO(getLogger(), "Previously stored scenes:"); + RCLCPP_INFO(node->get_logger(), "Previously stored scenes:"); for (const std::string& name : names) - RCLCPP_INFO(getLogger(), " * %s", name.c_str()); + RCLCPP_INFO(node->get_logger(), " * %s", name.c_str()); } cs.getKnownConstraints(names); if (names.empty()) { - RCLCPP_INFO(getLogger(), "There are no previously stored constraints"); + RCLCPP_INFO(node->get_logger(), "There are no previously stored constraints"); } else { - RCLCPP_INFO(getLogger(), "Previously stored constraints:"); + RCLCPP_INFO(node->get_logger(), "Previously stored constraints:"); for (const std::string& name : names) - RCLCPP_INFO(getLogger(), " * %s", name.c_str()); + RCLCPP_INFO(node->get_logger(), " * %s", name.c_str()); } rs.getKnownRobotStates(names); if (names.empty()) { - RCLCPP_INFO(getLogger(), "There are no previously stored robot states"); + RCLCPP_INFO(node->get_logger(), "There are no previously stored robot states"); } else { - RCLCPP_INFO(getLogger(), "Previously stored robot states:"); + RCLCPP_INFO(node->get_logger(), "Previously stored robot states:"); for (const std::string& name : names) - RCLCPP_INFO(getLogger(), " * %s", name.c_str()); + RCLCPP_INFO(node->get_logger(), " * %s", name.c_str()); } psm.addUpdateCallback([&](auto&&) { return onSceneUpdate(psm, pss); }); @@ -223,11 +226,11 @@ int main(int argc, char** argv) std::vector topics; psm.getMonitoredTopics(topics); - RCLCPP_INFO_STREAM(getLogger(), + RCLCPP_INFO_STREAM(node->get_logger(), "Listening for scene updates on topics " << fmt::format("{}", fmt::join(topics, ", "))); - RCLCPP_INFO_STREAM(getLogger(), "Listening for planning requests on topic " << mplan_req_sub->get_topic_name()); - RCLCPP_INFO_STREAM(getLogger(), "Listening for named constraints on topic " << constr_sub->get_topic_name()); - RCLCPP_INFO_STREAM(getLogger(), "Listening for states on topic " << state_sub->get_topic_name()); + RCLCPP_INFO_STREAM(node->get_logger(), "Listening for planning requests on topic " << mplan_req_sub->get_topic_name()); + RCLCPP_INFO_STREAM(node->get_logger(), "Listening for named constraints on topic " << constr_sub->get_topic_name()); + RCLCPP_INFO_STREAM(node->get_logger(), "Listening for states on topic " << state_sub->get_topic_name()); rclcpp::spin(node); return 0; diff --git a/moveit_ros/warehouse/src/state_storage.cpp b/moveit_ros/warehouse/src/state_storage.cpp index bbbfa81587..9661d65aa0 100644 --- a/moveit_ros/warehouse/src/state_storage.cpp +++ b/moveit_ros/warehouse/src/state_storage.cpp @@ -48,7 +48,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::RobotStateStorage::RobotStateStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::makeChildLogger("moveit_warehouse_robot_state_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_robot_state_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp index 009c162c7a..1551adcc64 100644 --- a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp @@ -49,8 +49,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)) - , logger_(moveit::makeChildLogger("moveit_warehouse_trajectory_constraints_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_trajectory_constraints_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/warehouse_connector.cpp b/moveit_ros/warehouse/src/warehouse_connector.cpp index 4c4521f61c..84193dce8a 100644 --- a/moveit_ros/warehouse/src/warehouse_connector.cpp +++ b/moveit_ros/warehouse/src/warehouse_connector.cpp @@ -42,14 +42,22 @@ #include #include +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("warehouse_connector"); +} +} // namespace + #ifdef _WIN32 void kill(int, int) { - RCLCPP_ERROR(moveit::getLogger(), "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(getLogger(), "Warehouse connector not supported on Windows"); } // Should never be called int fork() { - RCLCPP_ERROR(moveit::getLogger(), "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(getLogger(), "Warehouse connector not supported on Windows"); return -1; } #else @@ -58,8 +66,7 @@ int fork() namespace moveit_warehouse { -WarehouseConnector::WarehouseConnector(const std::string& dbexec) - : dbexec_(dbexec), child_pid_(0), logger_(moveit::makeChildLogger("moveit_warehouse_warehouse_connector")) +WarehouseConnector::WarehouseConnector(const std::string& dbexec) : dbexec_(dbexec), child_pid_(0) { } @@ -77,7 +84,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname) child_pid_ = fork(); if (child_pid_ == -1) { - RCLCPP_ERROR(logger_, "Error forking process."); + RCLCPP_ERROR(getLogger(), "Error forking process."); child_pid_ = 0; return false; } @@ -105,7 +112,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname) delete[] argv[1]; delete[] argv[2]; delete[] argv; - RCLCPP_ERROR_STREAM(logger_, + RCLCPP_ERROR_STREAM(getLogger(), "execv() returned " << code << ", errno=" << errno << " string errno = " << strerror(errno)); } return false; diff --git a/moveit_ros/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/src/warehouse_services.cpp index 747ee1d2ee..64c5a3afea 100644 --- a/moveit_ros/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/src/warehouse_services.cpp @@ -50,7 +50,13 @@ #include #include -using moveit::getLogger; +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("warehouse_services"); +} +} // namespace static const std::string ROBOT_DESCRIPTION = "robot_description"; @@ -141,7 +147,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("moveit_warehouse_services", node_options); - moveit::setLogger(node->get_logger()); + moveit::setNodeLoggerName(node->get_name()); std::string host; @@ -161,23 +167,23 @@ int main(int argc, char** argv) conn = moveit_warehouse::loadDatabase(node); conn->setParams(host, port, connection_timeout); - RCLCPP_INFO(getLogger(), "Connecting to warehouse on %s:%d", host.c_str(), port); + RCLCPP_INFO(node->get_logger(), "Connecting to warehouse on %s:%d", host.c_str(), port); int tries = 0; while (!conn->connect()) { ++tries; - RCLCPP_WARN(getLogger(), "Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries, + RCLCPP_WARN(node->get_logger(), "Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries, connection_retries); if (tries == connection_retries) { - RCLCPP_FATAL(getLogger(), "Failed to connect too many times, giving up"); + RCLCPP_FATAL(node->get_logger(), "Failed to connect too many times, giving up"); return 1; } } } catch (std::exception& ex) { - RCLCPP_ERROR(getLogger(), "%s", ex.what()); + RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); return 1; } @@ -187,13 +193,13 @@ int main(int argc, char** argv) rs.getKnownRobotStates(names); if (names.empty()) { - RCLCPP_INFO(getLogger(), "There are no previously stored robot states"); + RCLCPP_INFO(node->get_logger(), "There are no previously stored robot states"); } else { - RCLCPP_INFO(getLogger(), "Previously stored robot states:"); + RCLCPP_INFO(node->get_logger(), "Previously stored robot states:"); for (const std::string& name : names) - RCLCPP_INFO(getLogger(), " * %s", name.c_str()); + RCLCPP_INFO(node->get_logger(), " * %s", name.c_str()); } auto save_cb = [&](const std::shared_ptr& request, From 368c6178ab8cd27deed4ead627fc263a260eca1a Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 2 Nov 2023 16:55:38 -0600 Subject: [PATCH 4/8] update migration guide Signed-off-by: Tyler Weaver --- doc/MIGRATION_GUIDE.md | 34 +++------------------------------- 1 file changed, 3 insertions(+), 31 deletions(-) diff --git a/doc/MIGRATION_GUIDE.md b/doc/MIGRATION_GUIDE.md index 0ef76827b3..69657111d5 100644 --- a/doc/MIGRATION_GUIDE.md +++ b/doc/MIGRATION_GUIDE.md @@ -13,43 +13,15 @@ However because of initaliziation order (child logger has to be created after rc #include ``` -Wherever the node is created there is the option to set the global logger for moveit using this syntax: +Wherever the node is created there is the option to set the root logging namespace to be from the node for moveit using this syntax: ```C++ -moveit::setLogger(node->get_logger()); +moveit::setNodeLoggerName(node->get_name()); ``` Then wherever you call a logging macro you can use the `moveit::getLogger()` function: ```C++ -RCLCPP_INFO(moveit::getLogger(), "Very important info message"); -``` - -To have namespaces you need to create a child logger. -There is a function for that too. -This creates a child of the global logger. -You'll find this in the constructor of many of our classes. - -```C++ -, logger_(moveit::makeChildLogger("servo")) -``` - -Once you have a child logger you can use it in logging macros: -```C++ -RCLCPP_INFO(logger_, "Very important info message"); -``` - -In some files you'll find the creation of a static logger for the file like this. -Note that this is different from the previous file level static variables because the logger is not initialized until the function is called the first time. -This enables us to set the global node logger before this is called. -```C++ -namespace -{ -rclcpp::Logger getLogger() -{ - static auto logger = moveit::makeChildLogger("moveit_collision_detection"); - return logger; -} -} // namespace +RCLCPP_INFO(moveit::getLogger("my_namespace"), "Very important info message"); ``` ### Logger naming convention From e1c07098637657a1d5eef4d0e1b6811afec07689 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 27 Nov 2023 09:23:36 -0700 Subject: [PATCH 5/8] rebase Signed-off-by: Tyler Weaver --- .../src/check_for_stacked_constraints.cpp | 2 +- .../src/check_start_state_bounds.cpp | 2 +- .../src/check_start_state_collision.cpp | 2 +- .../src/add_ruckig_traj_smoothing.cpp | 2 +- .../src/display_motion_path.cpp | 2 +- .../planning_response_adapter_plugins/src/validate_path.cpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp index e33680723f..07f29b6cd9 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp @@ -52,7 +52,7 @@ namespace default_planning_request_adapters class CheckForStackedConstraints : public planning_interface::PlanningRequestAdapter { public: - CheckForStackedConstraints() : logger_(moveit::makeChildLogger("check_for_stacked_constraints")) + CheckForStackedConstraints() : logger_(moveit::getLogger("check_for_stacked_constraints")) { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp index c84cbcab74..0d938f2384 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp @@ -62,7 +62,7 @@ namespace default_planning_request_adapters class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter { public: - CheckStartStateBounds() : logger_(moveit::makeChildLogger("check_start_state_bounds")) + CheckStartStateBounds() : logger_(moveit::getLogger("check_start_state_bounds")) { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp index c2cd549504..f17c89f0b4 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp @@ -54,7 +54,7 @@ namespace default_planning_request_adapters class CheckStartStateCollision : public planning_interface::PlanningRequestAdapter { public: - CheckStartStateCollision() : logger_(moveit::makeChildLogger("validate_start_state")) + CheckStartStateCollision() : logger_(moveit::getLogger("validate_start_state")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index 9d133021dd..1034f95ff5 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -52,7 +52,7 @@ using namespace trajectory_processing; class AddRuckigTrajectorySmoothing : public planning_interface::PlanningResponseAdapter { public: - AddRuckigTrajectorySmoothing() : logger_(moveit::makeChildLogger("add_ruckig_trajectory_smoothing")) + AddRuckigTrajectorySmoothing() : logger_(moveit::getLogger("add_ruckig_trajectory_smoothing")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp index f2333e1102..9b2f98cff0 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp @@ -54,7 +54,7 @@ namespace default_planning_response_adapters class DisplayMotionPath : public planning_interface::PlanningResponseAdapter { public: - DisplayMotionPath() : logger_(moveit::makeChildLogger("display_motion_path")) + DisplayMotionPath() : logger_(moveit::getLogger("display_motion_path")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp index 8c4db824bc..d91b05c27a 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp @@ -52,7 +52,7 @@ namespace default_planning_response_adapters class ValidateSolution : public planning_interface::PlanningResponseAdapter { public: - ValidateSolution() : logger_(moveit::makeChildLogger("validate_solution")) + ValidateSolution() : logger_(moveit::getLogger("validate_solution")) { } From c5c7e8119435b7b38678f43dc8ec8d166d3c9ca5 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 27 Nov 2023 14:11:35 -0700 Subject: [PATCH 6/8] Fix clang-tidy wanrings --- .../src/collision_common.cpp | 4 ++++ moveit_core/robot_state/src/conversions.cpp | 2 ++ .../include/moveit/utils/moveit_error_code.h | 2 +- .../src/occupancy_map_server.cpp | 8 ++++---- .../src/planning_pipeline_interfaces.cpp | 19 +++++++++---------- .../src/move_group_interface.cpp | 4 +++- moveit_ros/warehouse/src/save_as_text.cpp | 2 +- 7 files changed, 24 insertions(+), 17 deletions(-) diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index f0ef932dc7..de0be9ce66 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -167,8 +167,10 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi return false; if (cdata->req_->verbose) + { RCLCPP_DEBUG(getLogger(), "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); + } // see if we need to compute a contact std::size_t want_contact_count{ 0 }; @@ -534,8 +536,10 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void return false; } if (cdata->req->verbose) + { RCLCPP_DEBUG(getLogger(), "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); + } double dist_threshold = cdata->req->distance_threshold; diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 59b95ca207..50b588e16b 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -359,8 +359,10 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { if (!state.clearAttachedBody(aco.object.id)) + { RCLCPP_ERROR(getLogger(), "The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str()); + } } else RCLCPP_ERROR(getLogger(), "Unknown collision object operation: %d", aco.object.operation); diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h index 3c55c6b927..9e4ff26f2b 100644 --- a/moveit_core/utils/include/moveit/utils/moveit_error_code.h +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -79,7 +79,7 @@ class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes @param error_code Error code to be translated to a string @return Error code string */ -inline std::string errorCodeToString(MoveItErrorCode error_code) +inline std::string errorCodeToString(const MoveItErrorCode& error_code) { switch (error_code.val) { diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index c0c095f493..1eaaf258cd 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -58,9 +58,10 @@ #include static void publishOctomap(const rclcpp::Publisher::SharedPtr& octree_binary_pub, - occupancy_map_monitor::OccupancyMapMonitor& server, rclcpp::Logger logger) + occupancy_map_monitor::OccupancyMapMonitor& server) { octomap_msgs::msg::Octomap map; + const auto logger = moveit::getLogger("occupancy_map_monitor"); map.header.frame_id = server.getMapFrame(); map.header.stamp = rclcpp::Clock().now(); @@ -100,9 +101,8 @@ int main(int argc, char** argv) std::shared_ptr buffer = std::make_shared(clock_ptr, tf2::durationFromSec(5.0)); tf2_ros::TransformListener listener(*buffer, node, false /* spin_thread - disables executor */); occupancy_map_monitor::OccupancyMapMonitor server(node, buffer); - server.setUpdateCallback([&octree_binary_pub, &server, logger = node->get_logger()] { - return publishOctomap(octree_binary_pub, server, logger); - }); + server.setUpdateCallback( + [&octree_binary_pub, &server, logger = node->get_logger()] { return publishOctomap(octree_binary_pub, server); }); server.startMonitor(); rclcpp::spin(node); diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index b4158fb054..23d6bd89f8 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -44,10 +44,9 @@ namespace moveit namespace planning_pipeline_interfaces { -rclcpp::Logger get_logger() +rclcpp::Logger getLogger() { - static rclcpp::Logger logger = moveit::getLogger("planning_pipeline_interfaces"); - return logger; + return moveit::getLogger("planning_pipeline_interfaces"); } ::planning_interface::MotionPlanResponse @@ -59,7 +58,7 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla auto it = planning_pipelines.find(motion_plan_request.pipeline_id); if (it == planning_pipelines.end()) { - RCLCPP_ERROR(get_logger(), "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); + RCLCPP_ERROR(getLogger(), "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; return motion_plan_response; } @@ -88,7 +87,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe const auto hardware_concurrency = std::thread::hardware_concurrency(); if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0) { - RCLCPP_WARN(get_logger(), + RCLCPP_WARN(getLogger(), "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " "hardware ('%d')", motion_plan_requests.size(), hardware_concurrency); @@ -106,7 +105,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::exception& e) { - RCLCPP_ERROR(get_logger(), "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); + RCLCPP_ERROR(getLogger(), "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); plan_solution = ::planning_interface::MotionPlanResponse(); plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; } @@ -118,7 +117,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe if (stopping_criterion_callback(plan_responses_container, motion_plan_requests)) { // Terminate planning pipelines - RCLCPP_INFO(get_logger(), "Stopping criterion met: Terminating planning pipelines that are still active"); + RCLCPP_INFO(getLogger(), "Stopping criterion met: Terminating planning pipelines that are still active"); for (const auto& request : motion_plan_requests) { try @@ -131,7 +130,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::out_of_range&) { - RCLCPP_WARN(get_logger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists", + RCLCPP_WARN(getLogger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists", request.pipeline_id.c_str()); } } @@ -174,7 +173,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, // Check if pipeline already exists if (planning_pipelines.count(planning_pipeline_name) > 0) { - RCLCPP_WARN(get_logger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_WARN(getLogger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } @@ -184,7 +183,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, if (!pipeline->getPlannerManager()) { - RCLCPP_ERROR(get_logger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 25aa2c9c5b..652569658a 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -1341,7 +1341,9 @@ MoveGroupInterface::~MoveGroupInterface() } MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) noexcept - : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_), logger_(other.logger_) + : remembered_joint_values_(std::move(other.remembered_joint_values_)) + , impl_(other.impl_) + , logger_(std::move(other.logger_)) { other.impl_ = nullptr; } diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index ae69f03ff6..72592f9e7c 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -57,7 +57,7 @@ typedef std::pair Lin typedef std::map LinkConstraintMap; void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap, - rclcpp::Logger logger) + const rclcpp::Logger& logger) { for (const moveit_msgs::msg::PositionConstraint& position_constraint : constraints.position_constraints) { From 3e3a6228e9e633d1c226435e71c9dcc53419d8df Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 4 Dec 2023 11:08:28 -0700 Subject: [PATCH 7/8] apply changes from review Signed-off-by: Tyler Weaver --- moveit_core/exceptions/src/exceptions.cpp | 5 +++-- .../robot_trajectory/test/test_robot_trajectory.cpp | 12 +++++------- moveit_core/utils/src/logger.cpp | 2 +- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 2 +- moveit_ros/benchmarks/src/BenchmarkOptions.cpp | 2 +- .../apply_planning_scene_service_capability.cpp | 2 +- .../clear_octomap_service_capability.cpp | 4 ++-- .../execute_trajectory_action_capability.cpp | 2 +- .../get_planning_scene_service_capability.cpp | 2 +- .../kinematics_service_capability.cpp | 2 +- .../default_capabilities/move_action_capability.cpp | 2 +- .../default_capabilities/plan_service_capability.cpp | 2 +- .../query_planners_service_capability.cpp | 2 +- .../state_validation_service_capability.cpp | 2 +- .../default_capabilities/tf_publisher_capability.cpp | 2 +- moveit_ros/move_group/src/move_group_capability.cpp | 2 +- moveit_ros/move_group/src/move_group_context.cpp | 2 +- moveit_ros/moveit_servo/src/utils/command.cpp | 2 +- 18 files changed, 25 insertions(+), 26 deletions(-) diff --git a/moveit_core/exceptions/src/exceptions.cpp b/moveit_core/exceptions/src/exceptions.cpp index ca0087bd9e..05cc9bc7ae 100644 --- a/moveit_core/exceptions/src/exceptions.cpp +++ b/moveit_core/exceptions/src/exceptions.cpp @@ -45,11 +45,12 @@ namespace moveit ConstructException::ConstructException(const std::string& what_arg) : std::runtime_error(what_arg) { - RCLCPP_ERROR(getLogger("exception"), "Error during construction of object: %s\nException thrown.", what_arg.c_str()); + RCLCPP_ERROR(getLogger("moveit_exception"), "Error during construction of object: %s\nException thrown.", + what_arg.c_str()); } Exception::Exception(const std::string& what_arg) : std::runtime_error(what_arg) { - RCLCPP_ERROR(getLogger("exception"), "%s\nException thrown.", what_arg.c_str()); + RCLCPP_ERROR(getLogger("moveit_exception"), "%s\nException thrown.", what_arg.c_str()); } } // namespace moveit diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index e663924c34..4a22a813ce 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -63,8 +63,6 @@ class RobotTrajectoryTestFixture : public testing::Test void TearDown() override { - robot_model_.reset(); - robot_state_.reset(); } void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) @@ -339,19 +337,19 @@ class OneRobot : public testing::Test trajectory = std::make_shared(robot_model_, arm_jmg_name_); - ASSERT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match"; - ASSERT_TRUE(trajectory->empty()) << "Generated trajectory not empty"; + EXPECT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match"; + EXPECT_TRUE(trajectory->empty()) << "Generated trajectory not empty"; double duration_from_previous = 0.1; std::size_t waypoint_count = 5; for (std::size_t ix = 0; ix < waypoint_count; ++ix) trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous); // Quick check that getDuration is working correctly - ASSERT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) + EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) << "Generated trajectory duration incorrect"; - ASSERT_EQ(waypoint_count, trajectory->getWayPointDurations().size()) + EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size()) << "Generated trajectory has the wrong number of waypoints"; - ASSERT_EQ(waypoint_count, trajectory->size()); + EXPECT_EQ(waypoint_count, trajectory->size()); } protected: diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index 33c5e26e60..992915662f 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -52,7 +52,7 @@ rclcpp::Logger& getGlobalRootLogger() // A random number is appended to the name used for the node to make it unique. // This unique node and logger name is only used if a user does not set a logger // through the `setNodeLoggerName` method to their node's logger. - auto name = fmt::format("____moveit_{}", rsl::rng()()); + auto name = fmt::format("moveit_{}", rsl::rng()()); try { static auto* moveit_node = new rclcpp::Node(name); diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index a646de69c0..09c811d2c9 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -68,7 +68,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("BenchmarkExecutor"); + return moveit::getLogger("benchmark_executor"); } } // namespace diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index b4f64e082d..54ca8fee0f 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("BenchmarkOptions"); + return moveit::getLogger("benchmark_options"); } } // namespace diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp index df7089e305..8ab96e122f 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp @@ -42,7 +42,7 @@ namespace move_group { -ApplyPlanningSceneService::ApplyPlanningSceneService() : MoveGroupCapability("ApplyPlanningSceneService") +ApplyPlanningSceneService::ApplyPlanningSceneService() : MoveGroupCapability("apply_planning_scene_service") { } diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp index af9231d74f..4af2520f3d 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp @@ -47,7 +47,7 @@ rclcpp::Logger getLogger() } } // namespace -move_group::ClearOctomapService::ClearOctomapService() : MoveGroupCapability("ClearOctomapService") +move_group::ClearOctomapService::ClearOctomapService() : MoveGroupCapability("clear_octomap_service") { } @@ -63,7 +63,7 @@ void move_group::ClearOctomapService::clearOctomap(const std::shared_ptr& /*res*/) { if (!context_->planning_scene_monitor_) - RCLCPP_ERROR(getLogger(), "Cannot clear octomap since planning_scene_monitor_ does not exist."); + RCLCPP_ERROR(getLogger(), "Cannot clear octomap since planning scene monitor has not been initialized."); RCLCPP_INFO(getLogger(), "Clearing octomap..."); context_->planning_scene_monitor_->clearOctomap(); diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index 8cf823e23e..582ec20696 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -53,7 +53,7 @@ rclcpp::Logger getLogger() } } // namespace -MoveGroupExecuteTrajectoryAction::MoveGroupExecuteTrajectoryAction() : MoveGroupCapability("ExecuteTrajectoryAction") +MoveGroupExecuteTrajectoryAction::MoveGroupExecuteTrajectoryAction() : MoveGroupCapability("execute_trajectory_action") { } diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp index 5379a45c71..09aac4eed2 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp @@ -39,7 +39,7 @@ namespace move_group { -MoveGroupGetPlanningSceneService::MoveGroupGetPlanningSceneService() : MoveGroupCapability("GetPlanningSceneService") +MoveGroupGetPlanningSceneService::MoveGroupGetPlanningSceneService() : MoveGroupCapability("get_planning_scene_service") { } diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 3cd14be915..f27073e6fa 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -45,7 +45,7 @@ namespace move_group { -MoveGroupKinematicsService::MoveGroupKinematicsService() : MoveGroupCapability("KinematicsService") +MoveGroupKinematicsService::MoveGroupKinematicsService() : MoveGroupCapability("kinematics_service") { } diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 34dba3fac0..c0cc597390 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -57,7 +57,7 @@ rclcpp::Logger getLogger() } // namespace MoveGroupMoveAction::MoveGroupMoveAction() - : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false } + : MoveGroupCapability("move_action"), move_state_(IDLE), preempt_requested_{ false } { } diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp index b6b59472c1..4a44fcdb18 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp @@ -52,7 +52,7 @@ rclcpp::Logger getLogger() } } // namespace -MoveGroupPlanService::MoveGroupPlanService() : MoveGroupCapability("MotionPlanService") +MoveGroupPlanService::MoveGroupPlanService() : MoveGroupCapability("motion_plan_service") { } diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index f6751eb0f4..76cf27ef1e 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -41,7 +41,7 @@ namespace move_group { -MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("QueryPlannersService") +MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("query_planners_service") { } diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp index 31db2ccf51..0322631b06 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp @@ -43,7 +43,7 @@ namespace move_group { -MoveGroupStateValidationService::MoveGroupStateValidationService() : MoveGroupCapability("StateValidationService") +MoveGroupStateValidationService::MoveGroupStateValidationService() : MoveGroupCapability("state_salidation_service") { } diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index 247749cef9..95925840d9 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -47,7 +47,7 @@ namespace move_group { -TfPublisher::TfPublisher() : MoveGroupCapability("TfPublisher") +TfPublisher::TfPublisher() : MoveGroupCapability("tf_publisher") { } diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index 66be57ddfd..aa8a1bdb9a 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -50,7 +50,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("MoveGroupCapability"); + return moveit::getLogger("move_group_capability"); } } // namespace diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index c7cabc81d9..cacc9c86df 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("MoveGroupContext"); + return moveit::getLogger("move_group_context"); } } // namespace diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index f88a64a64e..62a40cdd1e 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("servo_command"); + return moveit::getLogger("servo"); } /** From 2cec4efe3d11b5d52b74dbc060b8426e7dcf8d67 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 5 Dec 2023 09:34:02 -0700 Subject: [PATCH 8/8] Update moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../state_validation_service_capability.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp index 0322631b06..0e48096cb9 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp @@ -43,7 +43,7 @@ namespace move_group { -MoveGroupStateValidationService::MoveGroupStateValidationService() : MoveGroupCapability("state_salidation_service") +MoveGroupStateValidationService::MoveGroupStateValidationService() : MoveGroupCapability("state_validation_service") { }