From 86403e961ccd445d9a93d1c975c8c6b05de3ea10 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 9 Nov 2023 09:24:17 -0700 Subject: [PATCH] Revert "Use node logging in moveit_ros (#2482)" This reverts commit 73f4551d68f9e5d7109974743929efc0a940b315. --- doc/MIGRATION_GUIDE.md | 46 +++--- moveit_core/utils/CMakeLists.txt | 4 +- .../utils/include/moveit/utils/logger.hpp | 28 +--- moveit_core/utils/src/logger.cpp | 70 +++------ moveit_core/utils/test/CMakeLists.txt | 27 +--- moveit_core/utils/test/logger_dut.cpp | 2 +- .../utils/test/logger_from_child_dut.cpp | 2 +- .../utils/test/logger_from_only_child_dut.cpp | 53 ------- .../benchmarks/src/BenchmarkExecutor.cpp | 99 ++++++------- .../benchmarks/src/BenchmarkOptions.cpp | 75 +++++----- moveit_ros/benchmarks/src/RunBenchmark.cpp | 18 +-- .../src/global_planner_component.cpp | 18 +-- .../src/moveit_planning_pipeline.cpp | 15 +- .../src/hybrid_planning_manager.cpp | 39 ++--- .../src/forward_trajectory.cpp | 7 +- .../src/local_planner_component.cpp | 35 ++--- .../src/simple_sampler.cpp | 1 + ...pply_planning_scene_service_capability.cpp | 5 +- .../cartesian_path_service_capability.cpp | 18 +-- .../clear_octomap_service_capability.cpp | 10 +- .../execute_trajectory_action_capability.cpp | 13 +- .../kinematics_service_capability.cpp | 5 +- .../move_action_capability.cpp | 38 +++-- .../plan_service_capability.cpp | 8 +- .../tf_publisher_capability.cpp | 5 +- moveit_ros/move_group/src/move_group.cpp | 32 ++-- .../move_group/src/move_group_capability.cpp | 20 +-- .../move_group/src/move_group_context.cpp | 13 +- .../demos/cpp_interface/demo_joint_jog.cpp | 14 +- .../demos/cpp_interface/demo_pose.cpp | 14 +- .../demos/cpp_interface/demo_twist.cpp | 14 +- .../include/moveit_servo/servo.hpp | 2 - .../moveit_servo/src/collision_monitor.cpp | 12 +- moveit_ros/moveit_servo/src/servo.cpp | 65 ++++---- moveit_ros/moveit_servo/src/servo_node.cpp | 31 ++-- moveit_ros/moveit_servo/src/utils/command.cpp | 29 ++-- .../occupancy_map_monitor.h | 2 - ...ccupancy_map_monitor_middleware_handle.hpp | 1 - .../src/occupancy_map_monitor.cpp | 28 ++-- ...ccupancy_map_monitor_middleware_handle.cpp | 29 ++-- .../src/occupancy_map_server.cpp | 7 +- .../src/occupancy_map_updater.cpp | 6 +- .../depth_image_octomap_updater.h | 1 - .../src/depth_image_octomap_updater.cpp | 25 ++-- .../lazy_free_space_updater.h | 3 - .../src/lazy_free_space_updater.cpp | 19 ++- .../mesh_filter/src/gl_renderer.cpp | 5 +- .../point_containment_filter/CMakeLists.txt | 1 - .../src/shape_mask.cpp | 15 +- .../pointcloud_octomap_updater.h | 2 - .../src/pointcloud_octomap_updater.cpp | 17 +-- .../moveit/semantic_world/semantic_world.h | 2 - .../semantic_world/src/semantic_world.cpp | 32 ++-- .../collision_plugin_loader.h | 5 - .../src/collision_plugin_loader.cpp | 10 +- .../src/constraint_sampler_manager_loader.cpp | 13 +- .../src/list_planning_adapter_plugins.cpp | 2 - .../kinematics_plugin_loader.h | 4 +- .../src/kinematics_plugin_loader.cpp | 52 ++++--- .../include/moveit/moveit_cpp/moveit_cpp.h | 2 - .../moveit/moveit_cpp/planning_component.h | 1 - .../planning/moveit_cpp/src/moveit_cpp.cpp | 32 ++-- .../moveit_cpp/src/planning_component.cpp | 41 +++-- .../moveit/plan_execution/plan_execution.h | 3 - .../plan_execution/src/plan_execution.cpp | 45 +++--- .../src/display_random_state.cpp | 4 +- .../src/evaluate_collision_checking_speed.cpp | 14 +- .../src/print_planning_model_info.cpp | 2 - .../src/print_planning_scene_info.cpp | 7 +- .../src/publish_scene_from_text.cpp | 7 +- .../src/visualize_robot_collision_volume.cpp | 2 - .../planning_pipeline/planning_pipeline.h | 2 - .../src/planning_pipeline.cpp | 64 ++++---- .../src/planning_pipeline_interfaces.cpp | 22 +-- .../src/add_ruckig_traj_smoothing.cpp | 2 + .../src/add_time_optimal_parameterization.cpp | 12 +- .../src/fix_start_state_bounds.cpp | 13 +- .../src/fix_start_state_collision.cpp | 21 +-- .../src/fix_start_state_path_constraints.cpp | 21 +-- .../src/fix_workspace_bounds.cpp | 11 +- .../src/resolve_constraint_frames.cpp | 10 +- .../demos/demo_scene.cpp | 4 +- .../current_state_monitor.h | 2 - .../planning_scene_monitor.h | 2 - .../trajectory_monitor.h | 2 - .../src/current_state_monitor.cpp | 31 ++-- .../src/planning_scene_monitor.cpp | 123 ++++++++------- .../src/trajectory_monitor.cpp | 12 +- .../include/moveit/rdf_loader/rdf_loader.h | 2 - .../planning/rdf_loader/src/rdf_loader.cpp | 26 ++-- .../robot_model_loader/robot_model_loader.h | 1 - .../src/robot_model_loader.cpp | 37 +++-- .../trajectory_execution_manager.h | 1 - .../src/trajectory_execution_manager.cpp | 129 ++++++++-------- .../move_group_interface.h | 2 - .../src/move_group_interface.cpp | 140 +++++++++--------- .../src/planning_scene_interface.cpp | 16 +- .../robot_interaction/robot_interaction.h | 2 - .../src/interaction_handler.cpp | 8 +- .../src/kinematic_options.cpp | 5 +- .../src/robot_interaction.cpp | 46 +++--- .../motion_planning_frame.h | 2 - .../src/motion_planning_display.cpp | 4 +- .../src/motion_planning_frame.cpp | 23 ++- .../src/motion_planning_frame_context.cpp | 5 +- .../motion_planning_frame_joints_widget.cpp | 1 + .../motion_planning_frame_manipulation.cpp | 31 ++-- .../src/motion_planning_frame_objects.cpp | 35 ++--- .../src/motion_planning_frame_planning.cpp | 19 +-- .../src/motion_planning_frame_scenes.cpp | 1 + .../src/motion_planning_frame_states.cpp | 7 +- .../planning_scene_display.h | 1 - .../src/background_processing.cpp | 11 +- .../src/planning_scene_display.cpp | 11 +- .../trajectory_visualization.h | 2 - .../src/trajectory_visualization.cpp | 10 +- .../trajectory_display.h | 1 - .../src/trajectory_display.cpp | 7 +- moveit_ros/warehouse/src/broadcast.cpp | 2 +- moveit_ros/warehouse/src/import_from_text.cpp | 2 +- .../warehouse/src/initialize_demo_db.cpp | 2 +- moveit_ros/warehouse/src/save_as_text.cpp | 2 +- .../warehouse/src/save_to_warehouse.cpp | 2 +- .../warehouse/src/warehouse_services.cpp | 2 +- 124 files changed, 1019 insertions(+), 1261 deletions(-) delete mode 100644 moveit_core/utils/test/logger_from_only_child_dut.cpp diff --git a/doc/MIGRATION_GUIDE.md b/doc/MIGRATION_GUIDE.md index f528c173f7..36188f56b1 100644 --- a/doc/MIGRATION_GUIDE.md +++ b/doc/MIGRATION_GUIDE.md @@ -4,44 +4,36 @@ In MoveIt 1 we commonly use named ROS logging macros (i.e. `ROS_INFO_NAMED`) with file-specific logger names defined as `constexpr char LOGNAME[]="logger_name"`. ROS 2 provides similar macros (i.e. `RCLCPP_INFO`) that instead of an optional name always require a `rclcpp::Logger` instance for scoping the namespace. +All source files that use ROS logging should now define a file-specific `static const rclcpp::Logger` named `LOGGER`, located at the top of the file and inside the namespace with the narrowest scope (if there is one). +This leads to the following basic migration steps: -In ROS 2 logging is tied to the Node object for publishing to /rosout. -For namespaces there is the option of creating a child logger. -However because of initaliziation order (child logger has to be created after rclcpp::init is called and from a method on the logger from the node) you can no longer define the logger object as a file level static. -`moveit_core` provides a `util` library which contains some functions to make this situation more ergonomic. -```C++ -#include -``` +1. Replace `LOGNAME` with `rclcpp::Logger` instance: -Wherever the node is created there is the option to set the global logger for moveit using this syntax: + Old: -```C++ -moveit::setLogger(node->get_logger()); -``` + constexpr char LOGNAME[] = "logger_name"; -Then wherever you call a logging macro you can use the `moveit::getLogger()` function: -```C++ -RCLCPP_INFO(moveit::getLogger(), "Very important info message"); -``` + New: -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. + static const rclcpp::Logger LOGGER = rclcpp::get_logger("logger_name"); -```C++ -, logger_(moveit::makeChildLogger("servo")) -``` +2. Replace logging macros: -Once you have a child logger you can use it in logging macros: -```C++ -RCLCPP_INFO(logger_, "Very important info message"); -``` + Old: + + ROS_INFO_NAMED(LOGNAME, "Very important info message"); + + New: + + RCLCPP_INFO(LOGGER, "Very important info message"); ### Logger naming convention Migrating the loggers is a good opportunity to make logger names more consistent. In order to create unique and descriptive logger names we encourage the following naming pattern: general `LIBRARY_NAME.SOURCE_FILE_NAME`. +For instance, the file `joint_model_group.cpp` inside the library `moveit_robot_model` contains the following logger: + + static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.joint_model_group"); For finding the `LIBRARY_NAME` refer to the line `add_library(LIBRARY_NAME ...)` in the library's `CMakeLists.txt`. If the source file name is the same or very similar to the library name it is sufficient to only use the source file name. @@ -50,4 +42,4 @@ If the source file name is the same or very similar to the library name it is su Some classes declared in header files may contain log messages, for instance to warn about not-implemented virtual functions in abstract classes. A logger defined in the header file would not tell us what derived class is missing the implementation, since the source name would be resolved from the header file. -For this case, the base class should declare a private member variable `rclcpp::Logger logger_;` which is to be defined in the implementing class using the corresponding source file name. +For this case, the base class should declare a private member variable `static const rclcpp::Logger LOGGER` which is to be defined in the implementing class using the corresponding source file name. diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index a5d0a884f0..a206662a19 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -8,7 +8,7 @@ target_include_directories(moveit_utils PUBLIC $ $ ) -ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp rsl fmt) +ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp) set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -30,8 +30,6 @@ ament_target_dependencies(moveit_test_utils urdfdom urdfdom_headers rclcpp - rsl - fmt ) set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 1b0fe49255..fcd3e72b6d 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -42,30 +42,16 @@ 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"); -// ``` +// Function for getting a reference to a logger object kept in a global variable. +// Use getLoggerMut to set the logger to a node logger. 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. +// Function for getting a child logger. In Humble this also creates a node. +// Do not use this in place as it will create a new logger each time, +// instead store it in the state of your class or method. rclcpp::Logger makeChildLogger(const std::string& name); -// Set the global logger. -// Use: -// ```c++ -// moveit::setLogger(node->get_logger()); -// ``` -void setLogger(const rclcpp::Logger& logger); +// Mutable access to global logger for setting to node logger. +rclcpp::Logger& getLoggerMut(); } // namespace moveit diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index a01f73f76d..f444235d68 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -36,68 +36,42 @@ #include #include +#include +#include #include -#include -#include 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() -{ - 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()()); - try - { - static auto moveit_node = std::make_shared(name); - return moveit_node->get_logger(); - } - catch (const std::exception& ex) - { - // rclcpp::init was not called so rcl context is null, return non-node logger - auto logger = rclcpp::get_logger(name); - RCLCPP_WARN_STREAM(logger, "exception thrown while creating node for logging: " << ex.what()); - RCLCPP_WARN(logger, "if rclcpp::init was not called, messages from this logger may be missing from /rosout"); - return logger; - } - }(); - return logger; -} - const rclcpp::Logger& getLogger() { - return getGlobalLoggerRef(); + return getLoggerMut(); } rclcpp::Logger makeChildLogger(const std::string& name) { - return getGlobalLoggerRef().get_child(name); + // On versions of ROS older than Iron we need to create a node for each child logger + // Remove once Humble is EOL + // References: + // Use parent logger (rcl PR) - https://github.com/ros2/rcl/pull/921 + // Request for backport (rclpy issue) - https://github.com/ros2/rclpy/issues/1131 + // MoveIt PR that added this - https://github.com/ros-planning/moveit2/pull/2445 +#if !RCLCPP_VERSION_GTE(21, 0, 3) + static std::unordered_map> child_nodes; + if (child_nodes.find(name) == child_nodes.end()) + { + std::string ns = getLogger().get_name(); + child_nodes[name] = std::make_shared(name, ns); + } +#endif + + return getLoggerMut().get_child(name); } -void setLogger(const rclcpp::Logger& logger) +rclcpp::Logger& getLoggerMut() { - getGlobalLoggerRef() = logger; + static rclcpp::Logger logger = rclcpp::get_logger("moveit"); + return logger; } } // namespace moveit diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt index d216427c20..bb52a7e292 100644 --- a/moveit_core/utils/test/CMakeLists.txt +++ b/moveit_core/utils/test/CMakeLists.txt @@ -5,38 +5,21 @@ 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( TARGETS logger_dut logger_from_child_dut - logger_from_only_child_dut DESTINATION lib/${PROJECT_NAME} ) +# Add the launch tests 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. -if(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" - ) -endif() +add_launch_test(rosout_publish_test.py + TARGET test-node_logging_from_child + ARGS "dut:=logger_from_child_dut" +) diff --git a/moveit_core/utils/test/logger_dut.cpp b/moveit_core/utils/test/logger_dut.cpp index 1a7147321a..01201d62b3 100644 --- a/moveit_core/utils/test/logger_dut.cpp +++ b/moveit_core/utils/test/logger_dut.cpp @@ -44,7 +44,7 @@ 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::getLoggerMut() = node->get_logger(); // A node logger, should be in the file output and rosout auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), diff --git a/moveit_core/utils/test/logger_from_child_dut.cpp b/moveit_core/utils/test/logger_from_child_dut.cpp index f983e3b5eb..b051a98e30 100644 --- a/moveit_core/utils/test/logger_from_child_dut.cpp +++ b/moveit_core/utils/test/logger_from_child_dut.cpp @@ -44,7 +44,7 @@ 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::getLoggerMut() = node->get_logger(); // Make a child logger const auto child_logger = moveit::makeChildLogger("child"); 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 c261e8cddf..0000000000 --- a/moveit_core/utils/test/logger_from_only_child_dut.cpp +++ /dev/null @@ -1,53 +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 - -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::spin(node); -} diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 5de41d7724..cd4b0ce5c6 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -41,7 +41,6 @@ #include #include #include -#include // TODO(henningkayser): Switch to boost/timer/progress_display.hpp with Boost 1.72 // boost/progress.hpp is deprecated and will be replaced by boost/timer/progress_display.hpp in Boost 1.72. @@ -62,9 +61,10 @@ #undef max -using moveit::getLogger; using namespace moveit_ros_benchmarks; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.benchmarks.BenchmarkExecutor"); + template boost::posix_time::ptime toBoost(const std::chrono::time_point& from) { @@ -108,7 +108,7 @@ BenchmarkExecutor::~BenchmarkExecutor() { if (moveit_cpp_->getPlanningPipelines().find(planning_pipeline_name) == moveit_cpp_->getPlanningPipelines().end()) { - RCLCPP_ERROR(getLogger(), "Cannot find pipeline '%s'", planning_pipeline_name.c_str()); + RCLCPP_ERROR(LOGGER, "Cannot find pipeline '%s'", planning_pipeline_name.c_str()); return false; } @@ -116,7 +116,7 @@ BenchmarkExecutor::~BenchmarkExecutor() // Verify the pipeline has successfully initialized a planner if (!pipeline->getPlannerManager()) { - RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); + RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); continue; } } @@ -124,17 +124,14 @@ BenchmarkExecutor::~BenchmarkExecutor() // Error check if (moveit_cpp_->getPlanningPipelines().empty()) { - RCLCPP_ERROR(getLogger(), "No planning pipelines have been loaded. Nothing to do for the benchmarking service."); + RCLCPP_ERROR(LOGGER, "No planning pipelines have been loaded. Nothing to do for the benchmarking service."); } else { - RCLCPP_INFO(getLogger(), "Available planning pipelines:"); + RCLCPP_INFO(LOGGER, "Available planning pipelines:"); for (const std::pair& entry : moveit_cpp_->getPlanningPipelines()) - { - RCLCPP_INFO_STREAM(getLogger(), - "Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); - } + RCLCPP_INFO_STREAM(LOGGER, "Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); } return true; } @@ -205,7 +202,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& options) { if (moveit_cpp_->getPlanningPipelines().empty()) { - RCLCPP_ERROR(getLogger(), "No planning pipelines configured. Did you call BenchmarkExecutor::initialize?"); + RCLCPP_ERROR(LOGGER, "No planning pipelines configured. Did you call BenchmarkExecutor::initialize?"); return false; } @@ -237,7 +234,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& options) query_start_fn(queries[i].request, planning_scene_); } - RCLCPP_INFO(getLogger(), "Benchmarking query '%s' (%lu of %lu)", queries[i].name.c_str(), i + 1, queries.size()); + RCLCPP_INFO(LOGGER, "Benchmarking query '%s' (%lu of %lu)", queries[i].name.c_str(), i + 1, queries.size()); std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now(); runBenchmark(queries[i].request, options); std::chrono::duration dt = std::chrono::system_clock::now() - start_time; @@ -274,12 +271,12 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& options, if (!loadBenchmarkQueryData(options, scene_msg, start_states, path_constraints, goal_constraints, traj_constraints, queries)) { - RCLCPP_ERROR(getLogger(), "Failed to load benchmark query data"); + RCLCPP_ERROR(LOGGER, "Failed to load benchmark query data"); return false; } RCLCPP_INFO( - getLogger(), + LOGGER, "Benchmark loaded %lu starts, %lu goals, %lu path constraints, %lu trajectory constraints, and %lu queries", start_states.size(), goal_constraints.size(), path_constraints.size(), traj_constraints.size(), queries.size()); @@ -405,45 +402,45 @@ bool BenchmarkExecutor::loadBenchmarkQueryData( constraints_storage_ = std::make_shared(warehouse_connection); trajectory_constraints_storage_ = std::make_shared(warehouse_connection); - RCLCPP_INFO(getLogger(), "Connected to DB"); + RCLCPP_INFO(LOGGER, "Connected to DB"); } else { - RCLCPP_ERROR(getLogger(), "Failed to connect to DB"); + RCLCPP_ERROR(LOGGER, "Failed to connect to DB"); return false; } } catch (std::exception& e) { - RCLCPP_ERROR(getLogger(), "Failed to initialize benchmark server: '%s'", e.what()); + RCLCPP_ERROR(LOGGER, "Failed to initialize benchmark server: '%s'", e.what()); return false; } if (!loadPlanningScene(options.scene_name, scene_msg)) { - RCLCPP_ERROR(getLogger(), "Failed to load the planning scene"); + RCLCPP_ERROR(LOGGER, "Failed to load the planning scene"); return false; } if (!loadStates(options.start_state_regex, start_states)) { - RCLCPP_ERROR(getLogger(), "Failed to load the states"); + RCLCPP_ERROR(LOGGER, "Failed to load the states"); return false; } if (!loadPathConstraints(options.goal_constraint_regex, goal_constraints)) { - RCLCPP_ERROR(getLogger(), "Failed to load the goal constraints"); + RCLCPP_ERROR(LOGGER, "Failed to load the goal constraints"); } if (!loadPathConstraints(options.path_constraint_regex, path_constraints)) { - RCLCPP_ERROR(getLogger(), "Failed to load the path constraints"); + RCLCPP_ERROR(LOGGER, "Failed to load the path constraints"); } if (!loadTrajectoryConstraints(options.trajectory_constraint_regex, traj_constraints)) { - RCLCPP_ERROR(getLogger(), "Failed to load the trajectory constraints"); + RCLCPP_ERROR(LOGGER, "Failed to load the trajectory constraints"); } if (!loadQueries(options.query_regex, options.scene_name, queries)) { - RCLCPP_ERROR(getLogger(), "Failed to get a query regex"); + RCLCPP_ERROR(LOGGER, "Failed to get a query regex"); } return true; } @@ -538,7 +535,7 @@ bool BenchmarkExecutor::plannerConfigurationsExist( if (!pipeline_exists) { - RCLCPP_ERROR(getLogger(), "Planning pipeline '%s' does NOT exist", pipeline_config_entry.first.c_str()); + RCLCPP_ERROR(LOGGER, "Planning pipeline '%s' does NOT exist", pipeline_config_entry.first.c_str()); return false; } } @@ -568,8 +565,8 @@ bool BenchmarkExecutor::plannerConfigurationsExist( if (!planner_exists) { - RCLCPP_ERROR(getLogger(), "Planner '%s' does NOT exist for group '%s' in pipeline '%s'", - entry.second[i].c_str(), group_name.c_str(), entry.first.c_str()); + RCLCPP_ERROR(LOGGER, "Planner '%s' does NOT exist for group '%s' in pipeline '%s'", entry.second[i].c_str(), + group_name.c_str(), entry.first.c_str()); std::cout << "There are " << config_map.size() << " planner entries: " << '\n'; for (const auto& config_map_entry : config_map) std::cout << config_map_entry.second.name << '\n'; @@ -591,7 +588,7 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_ if (!planning_scene_storage_->getPlanningScene(planning_scene_w_metadata, scene_name)) { - RCLCPP_ERROR(getLogger(), "Failed to load planning scene '%s'", scene_name.c_str()); + RCLCPP_ERROR(LOGGER, "Failed to load planning scene '%s'", scene_name.c_str()); return false; } scene_msg = static_cast(*planning_scene_w_metadata); @@ -601,7 +598,7 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_ moveit_warehouse::PlanningSceneWorldWithMetadata pswwm; if (!planning_scene_world_storage_->getPlanningSceneWorld(pswwm, scene_name)) { - RCLCPP_ERROR(getLogger(), "Failed to load planning scene world '%s'", scene_name.c_str()); + RCLCPP_ERROR(LOGGER, "Failed to load planning scene world '%s'", scene_name.c_str()); return false; } scene_msg.world = static_cast(*pswwm); @@ -610,16 +607,16 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_ } else { - RCLCPP_ERROR(getLogger(), "Failed to find planning scene '%s'", scene_name.c_str()); + RCLCPP_ERROR(LOGGER, "Failed to find planning scene '%s'", scene_name.c_str()); return false; } } catch (std::exception& ex) { - RCLCPP_ERROR(getLogger(), "Error loading planning scene: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Error loading planning scene: %s", ex.what()); return false; } - RCLCPP_INFO(getLogger(), "Loaded planning scene successfully"); + RCLCPP_INFO(LOGGER, "Loaded planning scene successfully"); return true; } @@ -628,7 +625,7 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& { if (regex.empty()) { - RCLCPP_WARN(getLogger(), "No query regex provided, don't load any queries from the database"); + RCLCPP_WARN(LOGGER, "No query regex provided, don't load any queries from the database"); return true; } @@ -639,13 +636,13 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& } catch (std::exception& ex) { - RCLCPP_ERROR(getLogger(), "Error loading motion planning queries: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Error loading motion planning queries: %s", ex.what()); return false; } if (query_names.empty()) { - RCLCPP_ERROR(getLogger(), "Scene '%s' has no associated queries", scene_name.c_str()); + RCLCPP_ERROR(LOGGER, "Scene '%s' has no associated queries", scene_name.c_str()); return false; } @@ -658,7 +655,7 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& } catch (std::exception& ex) { - RCLCPP_ERROR(getLogger(), "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what()); + RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what()); continue; } @@ -667,7 +664,7 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& query.request = static_cast(*planning_query); queries.push_back(query); } - RCLCPP_INFO(getLogger(), "Loaded queries successfully"); + RCLCPP_INFO(LOGGER, "Loaded queries successfully"); return true; } @@ -681,7 +678,7 @@ bool BenchmarkExecutor::loadStates(const std::string& regex, std::vector& responses, const std::vector& solved) { - RCLCPP_INFO(getLogger(), "Computing result path similarity"); + RCLCPP_INFO(LOGGER, "Computing result path similarity"); const size_t result_count = planner_data.size(); size_t unsolved = std::count_if(solved.begin(), solved.end(), [](bool s) { return !s; }); std::vector average_distances(responses.size()); @@ -1239,7 +1236,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& benchmark_request, c std::ofstream out(filename.c_str()); if (!out) { - RCLCPP_ERROR(getLogger(), "Failed to open '%s' for benchmark output", filename.c_str()); + RCLCPP_ERROR(LOGGER, "Failed to open '%s' for benchmark output", filename.c_str()); return; } @@ -1377,5 +1374,5 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& benchmark_request, c } out.close(); - RCLCPP_INFO(getLogger(), "Benchmark results saved to '%s'", filename.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark results saved to '%s'", filename.c_str()); } diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index d381905edd..e348324540 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -35,11 +35,11 @@ /* Author: Ryan Luna */ #include -#include -using moveit::getLogger; using namespace moveit_ros_benchmarks; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.benchmarks.BenchmarkOptions"); + BenchmarkOptions::BenchmarkOptions(const rclcpp::Node::SharedPtr& node) { if (!readBenchmarkOptions(node)) @@ -58,12 +58,12 @@ bool BenchmarkOptions::readBenchmarkOptions(const rclcpp::Node::SharedPtr& node) if (!node->get_parameter("benchmark_config.warehouse.scene_name", scene_name)) { - RCLCPP_WARN(getLogger(), "Benchmark scene_name NOT specified"); + RCLCPP_WARN(LOGGER, "Benchmark scene_name NOT specified"); } - RCLCPP_INFO(getLogger(), "Benchmark host: %s", hostname.c_str()); - RCLCPP_INFO(getLogger(), "Benchmark port: %d", port); - RCLCPP_INFO(getLogger(), "Benchmark scene: %s", scene_name.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark host: %s", hostname.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark port: %d", port); + RCLCPP_INFO(LOGGER, "Benchmark scene: %s", scene_name.c_str()); // Read benchmark parameters node->get_parameter_or(std::string("benchmark_config.parameters.name"), benchmark_name, std::string("")); node->get_parameter_or(std::string("benchmark_config.parameters.runs"), runs, 10); @@ -85,7 +85,7 @@ bool BenchmarkOptions::readBenchmarkOptions(const rclcpp::Node::SharedPtr& node) if (!node->get_parameter(std::string("benchmark_config.parameters.group"), group_name)) { - RCLCPP_WARN(getLogger(), "Benchmark group NOT specified"); + RCLCPP_WARN(LOGGER, "Benchmark group NOT specified"); } if (node->has_parameter("benchmark_config.parameters.workspace")) @@ -94,7 +94,7 @@ bool BenchmarkOptions::readBenchmarkOptions(const rclcpp::Node::SharedPtr& node) // Make sure all params exist if (!node->get_parameter("benchmark_config.parameters.workspace.frame_id", workspace.header.frame_id)) { - RCLCPP_WARN(getLogger(), "Workspace frame_id not specified in benchmark config"); + RCLCPP_WARN(LOGGER, "Workspace frame_id not specified in benchmark config"); } node->get_parameter_or(std::string("benchmark_config.parameters.workspace.min_corner.x"), workspace.min_corner.x, @@ -122,22 +122,22 @@ bool BenchmarkOptions::readBenchmarkOptions(const rclcpp::Node::SharedPtr& node) node->get_parameter_or(std::string("benchmark_config.parameters.goal_offset.pitch"), goal_offsets.at(4), 0.0); node->get_parameter_or(std::string("benchmark_config.parameters.goal_offset.yaw"), goal_offsets.at(5), 0.0); - RCLCPP_INFO(getLogger(), "Benchmark name: '%s'", benchmark_name.c_str()); - RCLCPP_INFO(getLogger(), "Benchmark #runs: %d", runs); - RCLCPP_INFO(getLogger(), "Benchmark timeout: %f secs", timeout); - RCLCPP_INFO(getLogger(), "Benchmark group: %s", group_name.c_str()); - RCLCPP_INFO(getLogger(), "Benchmark query regex: '%s'", query_regex.c_str()); - RCLCPP_INFO(getLogger(), "Benchmark start state regex: '%s':", start_state_regex.c_str()); - RCLCPP_INFO(getLogger(), "Benchmark goal constraint regex: '%s':", goal_constraint_regex.c_str()); - RCLCPP_INFO(getLogger(), "Benchmark path constraint regex: '%s':", path_constraint_regex.c_str()); - RCLCPP_INFO(getLogger(), "Benchmark goal offsets (%f %f %f, %f %f %f)", goal_offsets.at(0), goal_offsets.at(1), + RCLCPP_INFO(LOGGER, "Benchmark name: '%s'", benchmark_name.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark #runs: %d", runs); + RCLCPP_INFO(LOGGER, "Benchmark timeout: %f secs", timeout); + RCLCPP_INFO(LOGGER, "Benchmark group: %s", group_name.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark query regex: '%s'", query_regex.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark start state regex: '%s':", start_state_regex.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark goal constraint regex: '%s':", goal_constraint_regex.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark path constraint regex: '%s':", path_constraint_regex.c_str()); + RCLCPP_INFO(LOGGER, "Benchmark goal offsets (%f %f %f, %f %f %f)", goal_offsets.at(0), goal_offsets.at(1), goal_offsets.at(2), goal_offsets.at(3), goal_offsets.at(4), goal_offsets.at(5)); - RCLCPP_INFO(getLogger(), "Benchmark output directory: %s", output_directory.c_str()); - RCLCPP_INFO_STREAM(getLogger(), "Benchmark workspace: min_corner: [" - << workspace.min_corner.x << ", " << workspace.min_corner.y << ", " - << workspace.min_corner.z << "], " - << "max_corner: [" << workspace.max_corner.x << ", " << workspace.max_corner.y - << ", " << workspace.max_corner.z << ']'); + RCLCPP_INFO(LOGGER, "Benchmark output directory: %s", output_directory.c_str()); + RCLCPP_INFO_STREAM(LOGGER, "Benchmark workspace: min_corner: [" + << workspace.min_corner.x << ", " << workspace.min_corner.y << ", " + << workspace.min_corner.z << "], " + << "max_corner: [" << workspace.max_corner.x << ", " << workspace.max_corner.y + << ", " << workspace.max_corner.z << ']'); // Read planner configuration if (!readPlannerConfigs(node)) { @@ -146,7 +146,7 @@ bool BenchmarkOptions::readBenchmarkOptions(const rclcpp::Node::SharedPtr& node) } else { - RCLCPP_ERROR(getLogger(), "No benchmark_config found on param server"); + RCLCPP_ERROR(LOGGER, "No benchmark_config found on param server"); return false; } return true; @@ -170,7 +170,7 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) std::vector pipelines; if (!node->get_parameter(ns + ".pipelines", pipelines)) { - RCLCPP_WARN(getLogger(), "No single planning pipeline namespace `%s` configured.", (ns + ".pipelines").c_str()); + RCLCPP_WARN(LOGGER, "No single planning pipeline namespace `%s` configured.", (ns + ".pipelines").c_str()); } for (const std::string& pipeline : pipelines) @@ -181,23 +181,23 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) const std::string pipeline_parameter_name = std::string(ns).append(".").append(pipeline).append(".name"); if (!node->get_parameter(pipeline_parameter_name, pipeline_name)) { - RCLCPP_ERROR(getLogger(), "Fail to get the parameter in `%s` namespace.", pipeline_parameter_name.c_str()); + RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_parameter_name.c_str()); return false; } - RCLCPP_INFO(getLogger(), "Reading in planner names for planning pipeline '%s'", pipeline_name.c_str()); + RCLCPP_INFO(LOGGER, "Reading in planner names for planning pipeline '%s'", pipeline_name.c_str()); std::vector planners; const std::string pipeline_parameter_planners = std::string(ns).append(".").append(pipeline).append(".planners"); if (!node->get_parameter(pipeline_parameter_planners, planners)) { - RCLCPP_ERROR(getLogger(), "Fail to get the parameter in `%s` namespace.", pipeline_parameter_planners.c_str()); + RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_parameter_planners.c_str()); return false; } for (const std::string& planner : planners) { - RCLCPP_INFO(getLogger(), " %s", planner.c_str()); + RCLCPP_INFO(LOGGER, " %s", planner.c_str()); } planning_pipelines[pipeline_name] = planners; @@ -209,7 +209,7 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) std::vector parallel_pipelines; if (!node->get_parameter(ns + ".parallel_pipelines", parallel_pipelines)) { - RCLCPP_WARN(getLogger(), "No parallel planning pipeline namespace `%s` configured.", + RCLCPP_WARN(LOGGER, "No parallel planning pipeline namespace `%s` configured.", (ns + ".parallel_pipelines").c_str()); } @@ -217,7 +217,7 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) { if (!parallel_pipeline.empty()) { // Read pipelines - RCLCPP_INFO(getLogger(), "Reading in parameters for parallel planning pipeline '%s'", parallel_pipeline.c_str()); + RCLCPP_INFO(LOGGER, "Reading in parameters for parallel planning pipeline '%s'", parallel_pipeline.c_str()); // Read pipelines std::vector pipelines; @@ -225,7 +225,7 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) std::string(ns).append(".").append(parallel_pipeline).append(".pipelines"); if (!node->get_parameter(pipelines_parameter, pipelines)) { - RCLCPP_ERROR(getLogger(), "Fail to get the parameter in `%s` namespace.", pipelines_parameter.c_str()); + RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", pipelines_parameter.c_str()); return false; } @@ -235,14 +235,13 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) std::string(ns).append(".").append(parallel_pipeline).append(".planner_ids"); if (!node->get_parameter(pipeline_planner_ids_parameter, planner_ids)) { - RCLCPP_ERROR(getLogger(), "Fail to get the parameter in `%s` namespace.", - pipeline_planner_ids_parameter.c_str()); + RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_planner_ids_parameter.c_str()); return false; } if (pipelines.size() != planner_ids.size()) { - RCLCPP_ERROR(getLogger(), "Number of planner ids is unequal to the number of pipelines in %s.", + RCLCPP_ERROR(LOGGER, "Number of planner ids is unequal to the number of pipelines in %s.", parallel_pipeline.c_str()); return false; } @@ -257,17 +256,17 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) for (const auto& entry : parallel_planning_pipelines) { - RCLCPP_INFO(getLogger(), "Parallel planning pipeline '%s'", entry.first.c_str()); + RCLCPP_INFO(LOGGER, "Parallel planning pipeline '%s'", entry.first.c_str()); for (const auto& pair : entry.second) { - RCLCPP_INFO(getLogger(), " '%s': '%s'", pair.first.c_str(), pair.second.c_str()); + RCLCPP_INFO(LOGGER, " '%s': '%s'", pair.first.c_str(), pair.second.c_str()); } } } } if (pipelines.empty() && parallel_pipelines.empty()) { - RCLCPP_ERROR(getLogger(), "No single or parallel planning pipelines configured for benchmarking."); + RCLCPP_ERROR(LOGGER, "No single or parallel planning pipelines configured for benchmarking."); return false; } return true; diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index ff68587b0f..d12952b168 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -44,9 +44,8 @@ #include #include #include -#include -using moveit::getLogger; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.benchmarks.RunBenchmark"); int main(int argc, char** argv) { @@ -55,7 +54,6 @@ 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()); // Read benchmark options from param server moveit_ros_benchmarks::BenchmarkOptions options(node); @@ -66,7 +64,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(LOGGER, "Failed to initialize benchmark server."); rclcpp::shutdown(); return 1; } @@ -83,18 +81,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(LOGGER, "Loaded scene names"); } else { - RCLCPP_ERROR(getLogger(), "Failed to load scene names from DB"); + RCLCPP_ERROR(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(LOGGER, "Failed to load scene names from DB: '%s'", e.what()); rclcpp::shutdown(); return 1; } @@ -104,7 +102,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(LOGGER, "Failed to run all benchmarks"); } } } @@ -112,11 +110,11 @@ int main(int argc, char** argv) { if (!server.runBenchmarks(options)) { - RCLCPP_ERROR(getLogger(), "Failed to run all benchmarks"); + RCLCPP_ERROR(LOGGER, "Failed to run all benchmarks"); } } - RCLCPP_INFO(getLogger(), "Finished benchmarking"); + RCLCPP_INFO(LOGGER, "Finished benchmarking"); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp index 804bc9bbdb..5b987e9f8c 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp @@ -42,6 +42,7 @@ namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("global_planner_component"); const auto JOIN_THREAD_TIMEOUT = std::chrono::seconds(1); } // namespace @@ -67,7 +68,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() node_->get_parameter("global_planning_action_name", global_planning_action_name); if (global_planning_action_name.empty()) { - RCLCPP_ERROR(node_->get_logger(), "global_planning_action_name was not defined"); + RCLCPP_ERROR(LOGGER, "global_planning_action_name was not defined"); return false; } cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -76,7 +77,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() // Goal callback [this](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(node_->get_logger(), "Received global planning goal request"); + RCLCPP_INFO(LOGGER, "Received global planning goal request"); // If another goal is active, cancel it and reject this goal if (long_callback_thread_.joinable()) { @@ -84,7 +85,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() auto future = std::async(std::launch::async, &std::thread::join, &long_callback_thread_); if (future.wait_for(JOIN_THREAD_TIMEOUT) == std::future_status::timeout) { - RCLCPP_WARN(node_->get_logger(), "Another goal was running. Rejecting the new hybrid planning goal."); + RCLCPP_WARN(LOGGER, "Another goal was running. Rejecting the new hybrid planning goal."); return rclcpp_action::GoalResponse::REJECT; } if (!global_planner_instance_->reset()) @@ -96,7 +97,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() }, // Cancel callback [this](const std::shared_ptr>& /*unused*/) { - RCLCPP_INFO(node_->get_logger(), "Received request to cancel global planning goal"); + RCLCPP_INFO(LOGGER, "Received request to cancel global planning goal"); if (long_callback_thread_.joinable()) { long_callback_thread_.join(); @@ -131,7 +132,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(node_->get_logger(), "Exception while creating global planner plugin loader: '%s'", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while creating global planner plugin loader: '%s'", ex.what()); return false; } try @@ -140,18 +141,17 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(node_->get_logger(), "Exception while loading global planner '%s': '%s'", planner_plugin_name_.c_str(), - ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while loading global planner '%s': '%s'", planner_plugin_name_.c_str(), ex.what()); return false; } // Initialize global planner plugin if (!global_planner_instance_->initialize(node_)) { - RCLCPP_ERROR(node_->get_logger(), "Unable to initialize global planner plugin '%s'", planner_plugin_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Unable to initialize global planner plugin '%s'", planner_plugin_name_.c_str()); return false; } - RCLCPP_INFO(node_->get_logger(), "Using global planner plugin '%s'", planner_plugin_name_.c_str()); + RCLCPP_INFO(LOGGER, "Using global planner plugin '%s'", planner_plugin_name_.c_str()); return true; } diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp index 5a487e8708..47d0d97e4a 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp @@ -36,6 +36,11 @@ #include #include +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("global_planner_component"); +} + namespace moveit::hybrid_planning { const std::string PLANNING_SCENE_MONITOR_NS = "planning_scene_monitor_options."; @@ -96,8 +101,7 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( if ((global_goal_handle->get_goal())->motion_sequence.items.empty()) { - RCLCPP_WARN(node_ptr_->get_logger(), - "Global planner received motion sequence request with no items. At least one is needed."); + RCLCPP_WARN(LOGGER, "Global planner received motion sequence request with no items. At least one is needed."); response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return response; } @@ -105,10 +109,9 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( // Process goal if ((global_goal_handle->get_goal())->motion_sequence.items.size() > 1) { - RCLCPP_WARN(node_ptr_->get_logger(), - "Global planner received motion sequence request with more than one item but the " - "'moveit_planning_pipeline' plugin only accepts one item. Just using the first item as global " - "planning goal!"); + RCLCPP_WARN(LOGGER, "Global planner received motion sequence request with more than one item but the " + "'moveit_planning_pipeline' plugin only accepts one item. Just using the first item as global " + "planning goal!"); } auto motion_plan_req = (global_goal_handle->get_goal())->motion_sequence.items[0].req; diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index 7b6044d6fe..b9a1426934 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -35,6 +35,11 @@ #include #include +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); +} // namespace + namespace moveit::hybrid_planning { using namespace std::chrono_literals; @@ -72,7 +77,7 @@ bool HybridPlanningManager::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(get_logger(), "Exception while creating planner logic plugin loader '%s'", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while creating planner logic plugin loader '%s'", ex.what()); } // TODO(sjahr) Refactor parameter declaration and use repository wide solution std::string logic_plugin_name = ""; @@ -92,11 +97,11 @@ bool HybridPlanningManager::initialize() { throw std::runtime_error("Unable to initialize planner logic plugin"); } - RCLCPP_INFO(get_logger(), "Using planner logic interface '%s'", logic_plugin_name.c_str()); + RCLCPP_INFO(LOGGER, "Using planner logic interface '%s'", logic_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(get_logger(), "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what()); } // Initialize local planning action client @@ -104,14 +109,14 @@ bool HybridPlanningManager::initialize() get_parameter("local_planning_action_name", local_planning_action_name); if (local_planning_action_name.empty()) { - RCLCPP_ERROR(get_logger(), "local_planning_action_name parameter was not defined"); + RCLCPP_ERROR(LOGGER, "local_planning_action_name parameter was not defined"); return false; } local_planner_action_client_ = rclcpp_action::create_client(this, local_planning_action_name); if (!local_planner_action_client_->wait_for_action_server(2s)) { - RCLCPP_ERROR(get_logger(), "Local planner action server not available after waiting"); + RCLCPP_ERROR(LOGGER, "Local planner action server not available after waiting"); return false; } @@ -120,14 +125,14 @@ bool HybridPlanningManager::initialize() get_parameter("global_planning_action_name", global_planning_action_name); if (global_planning_action_name.empty()) { - RCLCPP_ERROR(get_logger(), "global_planning_action_name parameter was not defined"); + RCLCPP_ERROR(LOGGER, "global_planning_action_name parameter was not defined"); return false; } global_planner_action_client_ = rclcpp_action::create_client(this, global_planning_action_name); if (!global_planner_action_client_->wait_for_action_server(2s)) { - RCLCPP_ERROR(get_logger(), "Global planner action server not available after waiting"); + RCLCPP_ERROR(LOGGER, "Global planner action server not available after waiting"); return false; } @@ -136,7 +141,7 @@ bool HybridPlanningManager::initialize() get_parameter("hybrid_planning_action_name", hybrid_planning_action_name); if (hybrid_planning_action_name.empty()) { - RCLCPP_ERROR(get_logger(), "hybrid_planning_action_name parameter was not defined"); + RCLCPP_ERROR(LOGGER, "hybrid_planning_action_name parameter was not defined"); return false; } cb_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -144,15 +149,15 @@ bool HybridPlanningManager::initialize() get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), hybrid_planning_action_name, // Goal callback - [this](const rclcpp_action::GoalUUID& /*unused*/, - const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(get_logger(), "Received goal request"); + [](const rclcpp_action::GoalUUID& /*unused*/, + const std::shared_ptr& /*unused*/) { + RCLCPP_INFO(LOGGER, "Received goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, // Cancel callback [this](const std::shared_ptr>& /*unused*/) { cancelHybridManagerGoals(); - RCLCPP_INFO(get_logger(), "Received request to cancel goal"); + RCLCPP_INFO(LOGGER, "Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; }, // Execution callback @@ -178,7 +183,7 @@ bool HybridPlanningManager::initialize() result->error_code.val = reaction_result.error_code.val; result->error_message = reaction_result.error_message; hybrid_planning_goal_handle_->abort(result); - RCLCPP_ERROR(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); } }); return true; @@ -216,7 +221,7 @@ void HybridPlanningManager::executeHybridPlannerGoal( result->error_code.val = reaction_result.error_code.val; result->error_message = reaction_result.error_message; hybrid_planning_goal_handle_->abort(result); - RCLCPP_ERROR_STREAM(get_logger(), "Hybrid Planning Manager failed to react to " << reaction_result.event); + RCLCPP_ERROR_STREAM(LOGGER, "Hybrid Planning Manager failed to react to " << reaction_result.event); } } @@ -267,7 +272,7 @@ bool HybridPlanningManager::sendGlobalPlannerAction() result->error_message = reaction_result.error_message; hybrid_planning_goal_handle_->abort(result); - RCLCPP_ERROR(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); } }; @@ -322,7 +327,7 @@ bool HybridPlanningManager::sendLocalPlannerAction() result->error_code.val = reaction_result.error_code.val; result->error_message = reaction_result.error_message; hybrid_planning_goal_handle_->abort(result); - RCLCPP_ERROR(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); } }; @@ -356,7 +361,7 @@ bool HybridPlanningManager::sendLocalPlannerAction() return result; }()); hybrid_planning_goal_handle_->abort(std::make_shared(result)); - RCLCPP_ERROR_STREAM(get_logger(), "Hybrid Planning Manager failed to react to " << reaction_result.event); + RCLCPP_ERROR_STREAM(LOGGER, "Hybrid Planning Manager failed to react to " << reaction_result.event); } }; diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp index 13086091bc..fe774dc01f 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -39,6 +39,7 @@ namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); // If stuck for this many iterations or more, abort the local planning action constexpr size_t STUCK_ITERATIONS_THRESHOLD = 5; constexpr double STUCK_THRESHOLD_RAD = 1e-4; // L1-norm sum across all joints @@ -84,7 +85,7 @@ ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajecto // A message every once in awhile is useful in case the local planner gets stuck #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 2000 /* ms */, "The local planner is solving..."); + RCLCPP_INFO_THROTTLE(LOGGER, *node_->get_clock(), 2000 /* ms */, "The local planner is solving..."); #pragma GCC diagnostic pop // Create controller command trajectory @@ -130,7 +131,7 @@ ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajecto feedback_result.feedback = toString(LocalFeedbackEnum::COLLISION_AHEAD); path_invalidation_event_send_ = true; // Set feedback flag } - RCLCPP_INFO(node_->get_logger(), "Collision ahead, holding current position"); + RCLCPP_INFO(LOGGER, "Collision ahead, holding current position"); // Keep current position moveit::core::RobotState current_state_command(*current_state); if (current_state_command.hasVelocities()) @@ -162,7 +163,7 @@ ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajecto prev_waypoint_target_ = nullptr; feedback_result.feedback = toString(LocalFeedbackEnum::LOCAL_PLANNER_STUCK); path_invalidation_event_send_ = true; // Set feedback flag - RCLCPP_INFO(node_->get_logger(), "The local planner has been stuck for several iterations. Aborting."); + RCLCPP_INFO(LOGGER, "The local planner has been stuck for several iterations. Aborting."); } } prev_waypoint_target_ = robot_command.getFirstWayPointPtr(); diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index fb91d7856f..bd1173b5a1 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -47,6 +47,7 @@ using namespace std::chrono_literals; namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); const auto JOIN_THREAD_TIMEOUT = std::chrono::seconds(1); // If the trajectory progress reaches more than 0.X the global goal state is considered as reached @@ -76,9 +77,8 @@ bool LocalPlannerComponent::initialize() if ((config_.publish_joint_positions && config_.publish_joint_velocities) || (!config_.publish_joint_positions && !config_.publish_joint_velocities)) { - RCLCPP_ERROR(node_->get_logger(), - "When publishing a std_msgs/Float64MultiArray, you must select positions OR velocities. " - "Enabling both or none is not possible!"); + RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, you must select positions OR velocities. " + "Enabling both or none is not possible!"); return false; } } @@ -88,7 +88,7 @@ bool LocalPlannerComponent::initialize() node_, "robot_description", "local_planner/planning_scene_monitor"); if (!planning_scene_monitor_->getPlanningScene()) { - RCLCPP_ERROR(node_->get_logger(), "Unable to configure planning scene monitor"); + RCLCPP_ERROR(LOGGER, "Unable to configure planning scene monitor"); return false; } @@ -107,7 +107,7 @@ bool LocalPlannerComponent::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(node_->get_logger(), "Exception while creating trajectory operator plugin loader: '%s'", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while creating trajectory operator plugin loader: '%s'", ex.what()); return false; } try @@ -117,12 +117,11 @@ bool LocalPlannerComponent::initialize() if (!trajectory_operator_instance_->initialize(node_, planning_scene_monitor_->getRobotModel(), config_.group_name)) // TODO(sjahr) add default group param throw std::runtime_error("Unable to initialize trajectory operator plugin"); - RCLCPP_INFO(node_->get_logger(), "Using trajectory operator interface '%s'", - config_.trajectory_operator_plugin_name.c_str()); + RCLCPP_INFO(LOGGER, "Using trajectory operator interface '%s'", config_.trajectory_operator_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(node_->get_logger(), "Exception while loading trajectory operator '%s': '%s'", + RCLCPP_ERROR(LOGGER, "Exception while loading trajectory operator '%s': '%s'", config_.trajectory_operator_plugin_name.c_str(), ex.what()); return false; } @@ -135,7 +134,7 @@ bool LocalPlannerComponent::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(node_->get_logger(), "Exception while creating constraint solver plugin loader '%s'", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while creating constraint solver plugin loader '%s'", ex.what()); return false; } try @@ -144,12 +143,11 @@ bool LocalPlannerComponent::initialize() local_constraint_solver_plugin_loader_->createUniqueInstance(config_.local_constraint_solver_plugin_name); if (!local_constraint_solver_instance_->initialize(node_, planning_scene_monitor_, config_.group_name)) throw std::runtime_error("Unable to initialize constraint solver plugin"); - RCLCPP_INFO(node_->get_logger(), "Using constraint solver interface '%s'", - config_.local_constraint_solver_plugin_name.c_str()); + RCLCPP_INFO(LOGGER, "Using constraint solver interface '%s'", config_.local_constraint_solver_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(node_->get_logger(), "Exception while loading constraint solver '%s': '%s'", + RCLCPP_ERROR(LOGGER, "Exception while loading constraint solver '%s': '%s'", config_.local_constraint_solver_plugin_name.c_str(), ex.what()); return false; } @@ -161,7 +159,7 @@ bool LocalPlannerComponent::initialize() // Goal callback [this](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(node_->get_logger(), "Received local planning goal request"); + RCLCPP_INFO(LOGGER, "Received local planning goal request"); // If another goal is active, cancel it and reject this goal if (long_callback_thread_.joinable()) { @@ -169,7 +167,7 @@ bool LocalPlannerComponent::initialize() auto future = std::async(std::launch::async, &std::thread::join, &long_callback_thread_); if (future.wait_for(JOIN_THREAD_TIMEOUT) == std::future_status::timeout) { - RCLCPP_WARN(node_->get_logger(), "Another goal was running. Rejecting the new hybrid planning goal."); + RCLCPP_WARN(LOGGER, "Another goal was running. Rejecting the new hybrid planning goal."); return rclcpp_action::GoalResponse::REJECT; } } @@ -177,7 +175,7 @@ bool LocalPlannerComponent::initialize() }, // Cancel callback [this](const std::shared_ptr>& /*unused*/) { - RCLCPP_INFO(node_->get_logger(), "Received request to cancel local planning goal"); + RCLCPP_INFO(LOGGER, "Received request to cancel local planning goal"); state_ = LocalPlannerState::ABORT; if (long_callback_thread_.joinable()) { @@ -226,7 +224,7 @@ bool LocalPlannerComponent::initialize() }); // Initialize local solution publisher - RCLCPP_INFO(node_->get_logger(), "Using '%s' as local solution topic type", config_.local_solution_topic_type.c_str()); + RCLCPP_INFO(LOGGER, "Using '%s' as local solution topic type", config_.local_solution_topic_type.c_str()); if (config_.local_solution_topic_type == "trajectory_msgs/JointTrajectory") { local_trajectory_publisher_ = @@ -294,7 +292,7 @@ void LocalPlannerComponent::executeIteration() if (!local_planner_feedback_->feedback.empty()) { local_planning_goal_handle_->publish_feedback(local_planner_feedback_); - RCLCPP_ERROR(node_->get_logger(), "Local planner somehow failed"); + RCLCPP_ERROR(LOGGER, "Local planner somehow failed"); reset(); return; } @@ -350,8 +348,7 @@ void LocalPlannerComponent::executeIteration() result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; result->error_message = "Unexpected failure."; local_planning_goal_handle_->abort(result); - RCLCPP_ERROR(node_->get_logger(), - "Local planner somehow failed"); // TODO(sjahr) Add more detailed failure information + RCLCPP_ERROR(LOGGER, "Local planner somehow failed"); // TODO(sjahr) Add more detailed failure information reset(); return; } diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp index ae3038ace1..b6cbacae06 100644 --- a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp @@ -40,6 +40,7 @@ namespace moveit::hybrid_planning { namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); constexpr double WAYPOINT_RADIAN_TOLERANCE = 0.2; // rad: L1-norm sum for all joints } // 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 160ff597bd..edbd907a09 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 @@ -37,10 +37,11 @@ #include "apply_planning_scene_service_capability.h" #include #include -#include namespace move_group { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_move_group_default_capabilities.apply_planning_scene_service_capability"); ApplyPlanningSceneService::ApplyPlanningSceneService() : MoveGroupCapability("ApplyPlanningSceneService") { @@ -67,7 +68,7 @@ bool ApplyPlanningSceneService::applyScene(const std::shared_ptrplanning_scene_monitor_) { - RCLCPP_ERROR(moveit::getLogger(), "Cannot apply PlanningScene as no scene is monitored."); + RCLCPP_ERROR(LOGGER, "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 0367ae0360..a892d17b09 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 @@ -46,9 +46,6 @@ #include #include #include -#include - -using moveit::getLogger; namespace { @@ -65,6 +62,9 @@ bool isStateValid(const planning_scene::PlanningScene* planning_scene, namespace move_group { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_move_group_default_capabilities.cartersian_path_service_capability"); + MoveGroupCartesianPathService::MoveGroupCartesianPathService() : MoveGroupCapability("CartesianPathService"), display_computed_paths_(true) { @@ -90,7 +90,7 @@ bool MoveGroupCartesianPathService::computeService( const std::shared_ptr& req, const std::shared_ptr& res) { - RCLCPP_INFO(getLogger(), "Received request to compute Cartesian path"); + RCLCPP_INFO(LOGGER, "Received request to compute Cartesian path"); context_->planning_scene_monitor_->updateFrameTransforms(); moveit::core::RobotState start_state = @@ -126,7 +126,7 @@ bool MoveGroupCartesianPathService::computeService( } else { - RCLCPP_ERROR(getLogger(), "Error encountered transforming waypoints to frame '%s'", default_frame.c_str()); + RCLCPP_ERROR(LOGGER, "Error encountered transforming waypoints to frame '%s'", default_frame.c_str()); ok = false; break; } @@ -137,8 +137,8 @@ bool MoveGroupCartesianPathService::computeService( { if (req->max_step < std::numeric_limits::epsilon()) { - RCLCPP_ERROR(getLogger(), "Maximum step to take between consecutive configurations along Cartesian path" - "was not specified (this value needs to be > 0)"); + RCLCPP_ERROR(LOGGER, "Maximum step to take between consecutive configurations along Cartesian path" + "was not specified (this value needs to be > 0)"); res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } else @@ -163,7 +163,7 @@ bool MoveGroupCartesianPathService::computeService( }; } bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req->header.frame_id); - RCLCPP_INFO(getLogger(), + RCLCPP_INFO(LOGGER, "Attempting to follow %u waypoints for link '%s' using a step of %lf m " "and jump threshold %lf (in %s reference frame)", static_cast(waypoints.size()), link_name.c_str(), req->max_step, @@ -185,7 +185,7 @@ bool MoveGroupCartesianPathService::computeService( time_param.computeTimeStamps(rt, req->max_velocity_scaling_factor, req->max_acceleration_scaling_factor); rt.getRobotTrajectoryMsg(res->solution); - RCLCPP_INFO(getLogger(), "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)", + RCLCPP_INFO(LOGGER, "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)", static_cast(traj.size()), res->fraction * 100.0); if (display_computed_paths_ && rt.getWayPointCount() > 0) { 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..7c18ed2a3c 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 @@ -37,7 +37,9 @@ #include "clear_octomap_service_capability.h" #include #include -#include + +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_move_group_default_capabilities.clear_octomap_service_capability"); move_group::ClearOctomapService::ClearOctomapService() : MoveGroupCapability("ClearOctomapService") { @@ -55,11 +57,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(LOGGER, "Cannot clear octomap since planning_scene_monitor_ does not exist."); - RCLCPP_INFO(moveit::getLogger(), "Clearing octomap..."); + RCLCPP_INFO(LOGGER, "Clearing octomap..."); context_->planning_scene_monitor_->clearOctomap(); - RCLCPP_INFO(moveit::getLogger(), "Octomap cleared."); + RCLCPP_INFO(LOGGER, "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..ff1bd1dac1 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 @@ -41,10 +41,11 @@ #include #include #include -#include namespace move_group { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_move_group_default_capabilities.execute_trajectory_action_capability"); MoveGroupExecuteTrajectoryAction::MoveGroupExecuteTrajectoryAction() : MoveGroupCapability("ExecuteTrajectoryAction") { @@ -61,9 +62,13 @@ void MoveGroupExecuteTrajectoryAction::initialize() node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), EXECUTE_ACTION_NAME, [](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { + RCLCPP_INFO(LOGGER, "Received goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [](const std::shared_ptr& /* unused */) { return rclcpp_action::CancelResponse::ACCEPT; }, + [](const std::shared_ptr& /* unused */) { + RCLCPP_INFO(LOGGER, "Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; + }, [this](const auto& goal) { executePathCallback(goal); }); } @@ -100,7 +105,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(LOGGER, "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 +129,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(LOGGER, "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..704c533ae1 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 @@ -40,10 +40,11 @@ #include #include #include -#include namespace move_group { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_move_group_default_capabilities.kinematics_service_capability"); MoveGroupKinematicsService::MoveGroupKinematicsService() : MoveGroupCapability("KinematicsService") { @@ -205,7 +206,7 @@ bool MoveGroupKinematicsService::computeFKService(const std::shared_ptrfk_link_names.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "No links specified for FK request"); + RCLCPP_ERROR(LOGGER, "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 289f69e5c9..b199bebc26 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 @@ -43,15 +43,13 @@ #include #include #include -#include - -using moveit::getLogger; namespace move_group { namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_default_capabilities.move_action_capability"); constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true; constexpr bool CHECK_SOLUTION_PATHS = true; } // namespace @@ -68,11 +66,11 @@ void MoveGroupMoveAction::initialize() execute_action_server_ = rclcpp_action::create_server( node, MOVE_ACTION, [](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(getLogger(), "MoveGroupMoveAction: Received request"); + RCLCPP_INFO(LOGGER, "Received request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, [this](const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(getLogger(), "MoveGroupMoveAction: Received request to cancel goal"); + RCLCPP_INFO(LOGGER, "Received request to cancel goal"); preemptMoveCallback(); return rclcpp_action::CancelResponse::ACCEPT; }, @@ -85,7 +83,7 @@ void MoveGroupMoveAction::initialize() void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr& goal) { goal_ = goal; - RCLCPP_INFO(getLogger(), "executing.."); + RCLCPP_INFO(LOGGER, "executing.."); setMoveState(PLANNING, goal_); // before we start planning, ensure that we have the latest robot state received... auto node = context_->moveit_cpp_->getNode(); @@ -97,9 +95,9 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptrget_goal()->planning_options.plan_only) { - RCLCPP_WARN(getLogger(), "This instance of MoveGroup is not allowed to execute trajectories " - "but the goal request has plan_only set to false. " - "Only a motion plan will be computed anyway."); + RCLCPP_WARN(LOGGER, "This instance of MoveGroup is not allowed to execute trajectories " + "but the goal request has plan_only set to false. " + "Only a motion plan will be computed anyway."); } executeMoveCallbackPlanOnly(goal, action_res); } @@ -108,8 +106,8 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptrplanned_trajectory); // @todo: Response messages - RCLCPP_INFO_STREAM(getLogger(), getActionResultString(action_res->error_code, planned_trajectory_empty, - goal->get_goal()->planning_options.plan_only)); + RCLCPP_INFO_STREAM(LOGGER, getActionResultString(action_res->error_code, planned_trajectory_empty, + goal->get_goal()->planning_options.plan_only)); if (action_res->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { goal->succeed(action_res); @@ -131,8 +129,8 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr& goal, std::shared_ptr& action_res) { - RCLCPP_INFO(getLogger(), "Combined planning and execution request received for MoveGroup action. " - "Forwarding to planning and execution pipeline."); + RCLCPP_INFO(LOGGER, "Combined planning and execution request received for MoveGroup action. " + "Forwarding to planning and execution pipeline."); if (moveit::core::isEmpty(goal->get_goal()->planning_options.planning_scene_diff)) { @@ -146,7 +144,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt current_state, kinematic_constraints::mergeConstraints(goal->get_goal()->request.goal_constraints[i], goal->get_goal()->request.path_constraints))) { - RCLCPP_INFO(getLogger(), "Goal constraints are already satisfied. No need to plan or execute any motions"); + RCLCPP_INFO(LOGGER, "Goal constraints are already satisfied. No need to plan or execute any motions"); action_res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return; } @@ -175,7 +173,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt plan_execution::ExecutableMotionPlan plan; if (preempt_requested_) { - RCLCPP_INFO(getLogger(), "Preempt requested before the goal is planned and executed."); + RCLCPP_INFO(LOGGER, "Preempt requested before the goal is planned and executed."); action_res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED; return; } @@ -191,7 +189,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr& goal, std::shared_ptr& action_res) { - RCLCPP_INFO(getLogger(), "Planning request received for MoveGroup action. Forwarding to planning pipeline."); + RCLCPP_INFO(LOGGER, "Planning request received for MoveGroup action. Forwarding to planning pipeline."); // lock the scene so that it does not modify the world representation while diff() is called planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); @@ -203,7 +201,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptrerror_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED; return; } @@ -222,13 +220,13 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptrgeneratePlan(the_scene, goal->get_goal()->request, res, context_->debug_, CHECK_SOLUTION_PATHS, DISPLAY_COMPUTED_MOTION_PLANS)) { - RCLCPP_ERROR(getLogger(), "Generating a plan with planning pipeline failed."); + RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } } catch (std::exception& ex) { - RCLCPP_ERROR(getLogger(), "Planning pipeline threw an exception: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } @@ -261,7 +259,7 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo } catch (std::exception& ex) { - RCLCPP_ERROR(getLogger(), "Planning pipeline threw an exception: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } if (res.trajectory) 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 4627d76159..39cb03e05d 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 @@ -39,12 +39,12 @@ #include #include #include -#include namespace move_group { namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_default_capabilities.plan_service_capability"); constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true; constexpr bool CHECK_SOLUTION_PATHS = true; } // namespace @@ -67,7 +67,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(LOGGER, "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()); @@ -89,14 +89,14 @@ bool MoveGroupPlanService::computePlanService(const std::shared_ptrgeneratePlan(ps, req->motion_plan_request, mp_res, context_->debug_, CHECK_SOLUTION_PATHS, DISPLAY_COMPUTED_MOTION_PLANS)) { - RCLCPP_ERROR(moveit::getLogger(), "Generating a plan with planning pipeline failed."); + RCLCPP_ERROR(LOGGER, "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(LOGGER, "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..06681b6180 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 @@ -42,10 +42,11 @@ #include #include #include -#include namespace move_group { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_move_group_default_capabilities.tf_publisher_capability"); TfPublisher::TfPublisher() : MoveGroupCapability("TfPublisher") { @@ -132,7 +133,7 @@ void TfPublisher::initialize() keep_running_ = true; - RCLCPP_INFO(moveit::getLogger(), "Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_); + RCLCPP_INFO(LOGGER, "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..429168efe0 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -44,11 +44,12 @@ #include #include #include -#include static const std::string ROBOT_DESCRIPTION = "robot_description"; // name of the robot description (a param name, so it can be changed externally) +static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group.move_group"); + namespace move_group { // These capabilities are loaded unless listed in disable_capabilities @@ -109,7 +110,7 @@ class MoveGroupExe } } else - RCLCPP_ERROR(moveit::getLogger(), "No MoveGroup context created. Nothing will work."); + RCLCPP_ERROR(LOGGER, "No MoveGroup context created. Nothing will work."); } MoveGroupContextPtr getContext() @@ -127,8 +128,7 @@ class MoveGroupExe } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL_STREAM(moveit::getLogger(), - "Exception while creating plugin loader for move_group capabilities: " << ex.what()); + RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating plugin loader for move_group capabilities: " << ex.what()); return; } @@ -182,7 +182,7 @@ class MoveGroupExe } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR_STREAM(moveit::getLogger(), + RCLCPP_ERROR_STREAM(LOGGER, "Exception while loading move_group capability '" << capability << "': " << ex.what()); } } @@ -195,7 +195,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(LOGGER, "%s", ss.str().c_str()); } MoveGroupContextPtr context_; @@ -212,7 +212,6 @@ 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_cpp::MoveItCpp::Options moveit_cpp_options(nh); // Prepare PlanningPipelineOptions @@ -222,7 +221,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(LOGGER, "Failed to read parameter 'move_group.planning_pipelines'"); } else { @@ -241,7 +240,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(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 +249,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(LOGGER, "MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default " "planning pipeline configuration"); } @@ -260,13 +259,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(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(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 +292,11 @@ int main(int argc, char** argv) debug = true; if (debug) { - RCLCPP_INFO(moveit::getLogger(), "MoveGroup debug mode is ON"); + RCLCPP_INFO(LOGGER, "MoveGroup debug mode is ON"); } else { - RCLCPP_INFO(moveit::getLogger(), "MoveGroup debug mode is OFF"); + RCLCPP_INFO(LOGGER, "MoveGroup debug mode is OFF"); } rclcpp::executors::MultiThreadedExecutor executor; @@ -308,7 +306,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(LOGGER, "MoveGroup monitors robot dynamics (higher load)"); planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); } @@ -321,7 +319,7 @@ int main(int argc, char** argv) rclcpp::shutdown(); } else - RCLCPP_ERROR(moveit::getLogger(), "Planning scene not configured"); + RCLCPP_ERROR(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..d3861d8b9c 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -39,11 +39,12 @@ #include #include #include -#include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_capabilities_base.move_group_capability"); + void move_group::MoveGroupCapability::setContext(const MoveGroupContextPtr& context) { context_ = context; @@ -88,14 +89,9 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vector 1) - { - RCLCPP_ERROR_STREAM(moveit::getLogger(), - "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!"); - } + RCLCPP_ERROR_STREAM(LOGGER, "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!"); if (!trajectory.empty()) - { convertToMsg(trajectory[0].trajectory, first_state_msg, trajectory_msg); - } } planning_interface::MotionPlanRequest @@ -104,7 +100,7 @@ move_group::MoveGroupCapability::clearRequestStartState(const planning_interface planning_interface::MotionPlanRequest r = request; r.start_state = moveit_msgs::msg::RobotState(); r.start_state.is_diff = true; - RCLCPP_WARN(moveit::getLogger(), + RCLCPP_WARN(LOGGER, "Execution of motions should always start at the robot's current state. Ignoring the state supplied as " "start state in the motion planning request"); return r; @@ -116,7 +112,7 @@ move_group::MoveGroupCapability::clearSceneRobotState(const moveit_msgs::msg::Pl moveit_msgs::msg::PlanningScene r = scene; r.robot_state = moveit_msgs::msg::RobotState(); r.robot_state.is_diff = true; - RCLCPP_WARN(moveit::getLogger(), + RCLCPP_WARN(LOGGER, "Execution of motions should always start at the robot's current state. Ignoring the state supplied as " "difference in the planning scene diff"); return r; @@ -204,7 +200,7 @@ bool move_group::MoveGroupCapability::performTransform(geometry_msgs::msg::PoseS } catch (tf2::TransformException& ex) { - RCLCPP_ERROR(moveit::getLogger(), "TF Problem: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "TF Problem: %s", ex.what()); return false; } return true; @@ -224,12 +220,12 @@ 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(LOGGER, "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(LOGGER, "Couldn't find requested planning pipeline '%s'", pipeline_id.c_str()); } } diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index 616df10bee..6c595893dd 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -39,7 +39,8 @@ #include #include #include -#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_capabilities_base.move_group_context"); move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, @@ -59,7 +60,7 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m else { RCLCPP_ERROR( - moveit::getLogger(), + LOGGER, "Failed to find default PlanningPipeline '%s' - please check MoveGroup's planning pipeline configuration.", default_planning_pipeline.c_str()); } @@ -85,15 +86,13 @@ bool move_group::MoveGroupContext::status() const const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager(); if (planner_interface) { - RCLCPP_INFO_STREAM(moveit::getLogger(), - "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); - RCLCPP_INFO_STREAM(moveit::getLogger(), "MoveGroup context initialization complete"); + RCLCPP_INFO_STREAM(LOGGER, "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); + RCLCPP_INFO_STREAM(LOGGER, "MoveGroup context initialization complete"); return true; } else { - RCLCPP_WARN_STREAM(moveit::getLogger(), - "MoveGroup running was unable to load " << planning_pipeline_->getPlannerPluginName()); + RCLCPP_WARN_STREAM(LOGGER, "MoveGroup running was unable to load " << planning_pipeline_->getPlannerPluginName()); return false; } } 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..237f3f715a 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 @@ -43,18 +43,20 @@ #include #include #include -#include -using moveit::getLogger; using namespace moveit_servo; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.joint_jog_demo"); +} + int main(int argc, char* argv[]) { rclcpp::init(argc, 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()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -88,7 +90,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(LOGGER, servo.getStatusMessage()); while (rclcpp::ok()) { const KinematicState joint_state = servo.getNextJointState(joint_jog); @@ -98,7 +100,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(LOGGER, "Timed out"); break; } else if (status != StatusCode::INVALID) @@ -108,6 +110,6 @@ int main(int argc, char* argv[]) rate.sleep(); } - RCLCPP_INFO(getLogger(), "Exiting demo."); + RCLCPP_INFO(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 1b3a7ae097..bfb5e92ad7 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -46,18 +46,20 @@ #include #include #include -#include -using moveit::getLogger; using namespace moveit_servo; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_demo"); +} + int main(int argc, char* argv[]) { rclcpp::init(argc, 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()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -124,7 +126,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(LOGGER, servo.getStatusMessage()); while (!stop_tracking && rclcpp::ok()) { @@ -146,12 +148,12 @@ int main(int argc, char* argv[]) command_rate.sleep(); } - RCLCPP_INFO_STREAM(getLogger(), "REACHED : " << stop_tracking); + RCLCPP_INFO_STREAM(LOGGER, "REACHED : " << stop_tracking); stop_tracking = true; if (tracker_thread.joinable()) tracker_thread.join(); - RCLCPP_INFO(getLogger(), "Exiting demo."); + RCLCPP_INFO(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 9a67ce7ed4..3bc53ca9fd 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -45,18 +45,20 @@ #include #include #include -#include -using moveit::getLogger; using namespace moveit_servo; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.twist_demo"); +} + int main(int argc, char* argv[]) { rclcpp::init(argc, 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()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -92,7 +94,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(LOGGER, servo.getStatusMessage()); while (rclcpp::ok()) { const KinematicState joint_state = servo.getNextJointState(target_twist); @@ -102,7 +104,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(LOGGER, "Timed out"); break; } else if (status != StatusCode::INVALID) @@ -112,6 +114,6 @@ int main(int argc, char* argv[]) rate.sleep(); } - RCLCPP_INFO(getLogger(), "Exiting demo."); + RCLCPP_INFO(LOGGER, "Exiting demo."); rclcpp::shutdown(); } diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index ec9ff168a2..b193e439fa 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -53,7 +53,6 @@ #include #include #include -#include namespace moveit_servo { @@ -207,7 +206,6 @@ class Servo servo::Params servo_params_; const rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; std::shared_ptr servo_param_listener_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp index 5a84a98a37..6d207798d4 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -39,7 +39,11 @@ #include #include -#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); +} namespace moveit_servo { @@ -63,11 +67,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(LOGGER, "Collision monitor started"); } else { - RCLCPP_ERROR_STREAM(moveit::getLogger(), "Collision monitor could not be started"); + RCLCPP_ERROR_STREAM(LOGGER, "Collision monitor could not be started"); } } @@ -78,7 +82,7 @@ void CollisionMonitor::stop() { monitor_thread_.join(); } - RCLCPP_INFO_STREAM(moveit::getLogger(), "Collision monitor stopped"); + RCLCPP_INFO_STREAM(LOGGER, "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 2df47d8eed..d32c3d427b 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -41,13 +41,13 @@ #include #include #include -#include // Disable -Wold-style-cast because all _THROTTLE macros trigger this #pragma GCC diagnostic ignored "-Wold-style-cast" namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); constexpr double ROBOT_STATE_WAIT_TIME = 5.0; // seconds constexpr double STOPPED_VELOCITY_EPS = 1e-4; } // namespace @@ -58,7 +58,6 @@ 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")) , servo_param_listener_{ std::move(servo_param_listener) } , planning_scene_monitor_{ planning_scene_monitor } { @@ -66,14 +65,14 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptrgetStateMonitor()->waitForCompleteState(servo_params_.move_group_name, ROBOT_STATE_WAIT_TIME)) { - RCLCPP_ERROR(logger_, "Timeout waiting for current state"); + RCLCPP_ERROR(LOGGER, "Timeout waiting for current state"); std::exit(EXIT_FAILURE); } @@ -92,12 +91,12 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptrknowsFrameTransform(servo_params_.planning_frame)) { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "No transform available for planning frame " << servo_params_.planning_frame); + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for planning frame " << servo_params_.planning_frame); } else if (!robot_state->knowsFrameTransform(servo_params_.ee_frame)) { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "No transform available for end effector frame " << servo_params_.ee_frame); + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for end effector frame " << servo_params_.ee_frame); } else { @@ -108,7 +107,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr(std::string(sub_group_name), std::move(new_map))); } - RCLCPP_INFO_STREAM(logger_, "Servo initialized successfully"); + RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully"); } Servo::~Servo() @@ -167,7 +166,7 @@ void Servo::setSmoothingPlugin() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(logger_, "Exception while loading the smoothing plugin '%s': '%s'", + RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", servo_params_.smoothing_filter_plugin_name.c_str(), ex.what()); std::exit(EXIT_FAILURE); } @@ -178,7 +177,7 @@ void Servo::setSmoothingPlugin() robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size(); if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints)) { - RCLCPP_ERROR(logger_, "Smoothing plugin could not be initialized"); + RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); std::exit(EXIT_FAILURE); } const KinematicState current_state = getCurrentRobotState(); @@ -197,49 +196,49 @@ bool Servo::validateParams(const servo::Params& servo_params) const auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); if (joint_model_group == nullptr) { - RCLCPP_ERROR_STREAM(logger_, "Invalid move group name: `" << servo_params.move_group_name << '`'); + RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params.move_group_name << '`'); params_valid = false; } if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold) { - RCLCPP_ERROR(logger_, "Parameter 'hard_stop_singularity_threshold' " - "should be greater than 'lower_singularity_threshold.' " - "Check the parameters YAML file used to launch this node."); + RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' " + "should be greater than 'lower_singularity_threshold.' " + "Check the parameters YAML file used to launch this node."); params_valid = false; } if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities && !servo_params.publish_joint_accelerations) { - RCLCPP_ERROR(logger_, "At least one of publish_joint_positions / " - "publish_joint_velocities / " - "publish_joint_accelerations must be true. " - "Check the parameters YAML file used to launch this node."); + RCLCPP_ERROR(LOGGER, "At least one of publish_joint_positions / " + "publish_joint_velocities / " + "publish_joint_accelerations must be true. " + "Check the parameters YAML file used to launch this node."); params_valid = false; } if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions && servo_params.publish_joint_velocities) { - RCLCPP_ERROR(logger_, "When publishing a std_msgs/Float64MultiArray, " - "you must select positions OR velocities." - "Check the parameters YAML file used to launch this node."); + RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, " + "you must select positions OR velocities." + "Check the parameters YAML file used to launch this node."); params_valid = false; } if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold) { - RCLCPP_ERROR(logger_, "Parameter 'self_collision_proximity_threshold' should probably be less " - "than or equal to 'scene_collision_proximity_threshold'." - "Check the parameters YAML file used to launch this node."); + RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " + "than or equal to 'scene_collision_proximity_threshold'." + "Check the parameters YAML file used to launch this node."); params_valid = false; } if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name && !joint_model_group->isSubgroup(servo_params.active_subgroup)) { - RCLCPP_ERROR(logger_, + RCLCPP_ERROR(LOGGER, "The value '%s' Parameter 'active_subgroup' does not name a valid subgroup of joint group '%s'.", servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str()); params_valid = false; @@ -259,8 +258,8 @@ bool Servo::updateParams() { if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor) { - RCLCPP_INFO_STREAM(logger_, "override_velocity_scaling_factor changed to : " - << std::to_string(params.override_velocity_scaling_factor)); + RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : " + << std::to_string(params.override_velocity_scaling_factor)); } servo_params_ = params; @@ -268,7 +267,7 @@ bool Servo::updateParams() } else { - RCLCPP_WARN_STREAM(logger_, "Parameters will not be updated."); + RCLCPP_WARN_STREAM(LOGGER, "Parameters will not be updated."); } } return params_updated; @@ -315,7 +314,7 @@ KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const K { halting_joint_names << bounded_state.joint_names[idx] + " "; } - RCLCPP_WARN_STREAM(logger_, "Joint position limit reached on joints: " << halting_joint_names.str()); + RCLCPP_WARN_STREAM(LOGGER, "Joint position limit reached on joints: " << halting_joint_names.str()); const bool all_joint_halt = (getCommandType() == CommandType::JOINT_JOG && servo_params_.halt_all_joints_in_joint_mode) || @@ -377,7 +376,7 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo catch (tf2::TransformException& ex) { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "Could not transform twist to planning frame."); + RCLCPP_ERROR_STREAM(LOGGER, "Could not transform twist to planning frame."); } } else if (expected_type == CommandType::POSE) @@ -392,7 +391,7 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo catch (tf2::TransformException& ex) { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "Could not transform pose to planning frame."); + RCLCPP_ERROR_STREAM(LOGGER, "Could not transform pose to planning frame."); } } @@ -404,7 +403,7 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo else { servo_status_ = StatusCode::INVALID; - RCLCPP_WARN_STREAM(logger_, "Incoming servo command type does not match known command types."); + RCLCPP_WARN_STREAM(LOGGER, "Incoming servo command type does not match known command types."); } return joint_position_deltas; @@ -474,7 +473,7 @@ KinematicState Servo::getNextJointState(const ServoInput& command) servo_params_.override_velocity_scaling_factor); if (joint_limit_scale < 1.0) // 1.0 means no scaling. { - RCLCPP_DEBUG_STREAM(logger_, "Joint velocity limit scaling applied by a factor of " << joint_limit_scale); + RCLCPP_DEBUG_STREAM(LOGGER, "Joint velocity limit scaling applied by a factor of " << joint_limit_scale); } target_state.velocities *= joint_limit_scale; diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 4b23d1aba9..02dd61bce9 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -40,9 +40,11 @@ #include #include -#include -using moveit::getLogger; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); +} // namespace namespace moveit_servo { @@ -67,14 +69,11 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) , new_twist_msg_{ false } , new_pose_msg_{ false } { - moveit::setLogger(node_->get_logger()); - if (!options.use_intra_process_comms()) { - RCLCPP_WARN_STREAM(getLogger(), - "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"); + RCLCPP_WARN_STREAM(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"); } // Check if a realtime kernel is available @@ -82,16 +81,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(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(LOGGER, "Could not enable FIFO RT scheduling policy."); } } else { - RCLCPP_WARN_STREAM(getLogger(), "Realtime kernel is recommended for better performance."); + RCLCPP_WARN_STREAM(LOGGER, "Realtime kernel is recommended for better performance."); } std::shared_ptr servo_param_listener = @@ -178,7 +177,7 @@ void ServoNode::switchCommandType(const std::shared_ptrcommand_type << " requested"); + RCLCPP_WARN_STREAM(LOGGER, "Unknown command type " << request->command_type << " requested"); } response->success = (request->command_type == static_cast(servo_->getCommandType())); } @@ -221,7 +220,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(LOGGER, "Joint jog command timed out. Halting to a stop."); } } @@ -253,7 +252,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(LOGGER, "Twist command timed out. Halting to a stop."); } } @@ -282,7 +281,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(LOGGER, "Pose command timed out. Halting to a stop."); } } @@ -319,7 +318,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(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 63b7dde8c5..9ddfbda8b0 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -38,12 +38,10 @@ */ #include -#include - -using moveit::getLogger; namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.command_processor"); /** * @brief Helper function to create a move group deltas vector from a sub group deltas vector. A delta vector for the @@ -102,7 +100,7 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo } else { - RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i]); + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint name: " << command.names[i]); names_valid = false; break; @@ -122,14 +120,13 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo status = StatusCode::INVALID; if (!names_valid) { - RCLCPP_WARN_STREAM(getLogger(), - "Invalid joint names in joint jog command. Either you're sending commands for a joint " - "that is not part of the move group or certain joints cannot be moved because a " - "subgroup is active and they are not part of it."); + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint names in joint jog command. Either you're sending commands for a joint " + "that is not part of the move group or certain joints cannot be moved because a " + "subgroup is active and they are not part of it."); } if (!velocity_valid) { - RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command"); + RCLCPP_WARN_STREAM(LOGGER, "Invalid velocity values in joint jog command"); } } @@ -182,7 +179,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: if (singularity_scaling_info.second != StatusCode::NO_WARNING) { status = singularity_scaling_info.second; - RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status)); + RCLCPP_WARN_STREAM(LOGGER, SERVO_STATUS_CODE_MAP.at(status)); joint_position_delta *= singularity_scaling_info.first; } } @@ -196,11 +193,11 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: status = StatusCode::INVALID; if (!valid_command) { - RCLCPP_WARN_STREAM(getLogger(), "Invalid twist command."); + RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command."); } if (!is_planning_frame) { - RCLCPP_WARN_STREAM(getLogger(), + RCLCPP_WARN_STREAM(LOGGER, "Command frame is: " << command.frame_id << ", expected: " << servo_params.planning_frame); } } @@ -246,11 +243,11 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co status = StatusCode::INVALID; if (!valid_command) { - RCLCPP_WARN_STREAM(getLogger(), "Invalid pose command."); + RCLCPP_WARN_STREAM(LOGGER, "Invalid pose command."); } if (!is_planning_frame) { - RCLCPP_WARN_STREAM(getLogger(), + RCLCPP_WARN_STREAM(LOGGER, "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame); } } @@ -279,7 +276,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt if (!ik_solver_supports_group) { status = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(getLogger(), "Loaded IK plugin does not support group " << joint_model_group->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "Loaded IK plugin does not support group " << joint_model_group->getName()); } } @@ -310,7 +307,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt else { status = StatusCode::INVALID; - RCLCPP_WARN_STREAM(getLogger(), "Could not find IK solution for requested motion, got error code " << err.val); + RCLCPP_WARN_STREAM(LOGGER, "Could not find IK solution for requested motion, got error code " << err.val); } } else diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h index 8b34da6d8a..cb817e1486 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h @@ -329,7 +329,5 @@ class OccupancyMapMonitor std::size_t mesh_handle_count_; /*!< Count of mesh handles */ bool active_; /*!< True when actively monitoring updaters */ - - rclcpp::Logger logger_; }; } // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp index d73e7c43a7..9d85099bc7 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp @@ -110,7 +110,6 @@ class OccupancyMapMonitorMiddlewareHandle : public OccupancyMapMonitor::Middlewa updater_plugin_loader_; /*!< Pluginlib loader for OccupancyMapUpdater */ OccupancyMapMonitor::Parameters parameters_; /*!< ROS parameters for OccupancyMapMonitor */ - rclcpp::Logger logger_; }; } // namespace occupancy_map_monitor 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..21fd7c818f 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -47,10 +47,13 @@ #include #include #include -#include namespace occupancy_map_monitor { +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_monitor"); +} OccupancyMapMonitor::OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, double map_resolution) : OccupancyMapMonitor{ std::make_unique(node, map_resolution, ""), nullptr } @@ -73,7 +76,6 @@ OccupancyMapMonitor::OccupancyMapMonitor(std::unique_ptr middl , debug_info_{ false } , mesh_handle_count_{ 0 } , active_{ false } - , logger_(moveit::makeChildLogger("occupancy_map_monitor")) { if (middleware_handle_ == nullptr) { @@ -83,16 +85,16 @@ OccupancyMapMonitor::OccupancyMapMonitor(std::unique_ptr middl // Get the parameters parameters_ = middleware_handle_->getParameters(); - RCLCPP_DEBUG(logger_, "Using resolution = %lf m for building octomap", parameters_.map_resolution); + RCLCPP_DEBUG(LOGGER, "Using resolution = %lf m for building octomap", parameters_.map_resolution); if (tf_buffer_ != nullptr && parameters_.map_frame.empty()) { - RCLCPP_WARN(logger_, "No target frame specified for Octomap. No transforms will be applied to received data."); + RCLCPP_WARN(LOGGER, "No target frame specified for Octomap. No transforms will be applied to received data."); } if (tf_buffer_ == nullptr && !parameters_.map_frame.empty()) { - RCLCPP_WARN(logger_, "Target frame specified but no TF instance (buffer) specified." - "No transforms will be applied to received data."); + RCLCPP_WARN(LOGGER, "Target frame specified but no TF instance (buffer) specified." + "No transforms will be applied to received data."); } tree_ = std::make_shared(parameters_.map_resolution); @@ -105,7 +107,7 @@ OccupancyMapMonitor::OccupancyMapMonitor(std::unique_ptr middl // Verify the updater was loaded if (occupancy_map_updater == nullptr) { - RCLCPP_ERROR_STREAM(logger_, "Failed to load sensor: `" << sensor_name << "` of type: `" << sensor_type << '`'); + RCLCPP_ERROR_STREAM(LOGGER, "Failed to load sensor: `" << sensor_name << "` of type: `" << sensor_type << '`'); continue; } @@ -118,7 +120,7 @@ OccupancyMapMonitor::OccupancyMapMonitor(std::unique_ptr middl // Load the params in the updater if (!occupancy_map_updater->setParams(sensor_name)) { - RCLCPP_ERROR_STREAM(logger_, "Failed to configure updater of type " << occupancy_map_updater->getType()); + RCLCPP_ERROR_STREAM(LOGGER, "Failed to configure updater of type " << occupancy_map_updater->getType()); continue; } @@ -177,7 +179,7 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) updater->setTransformCacheCallback(transform_cache_callback_); } else - RCLCPP_ERROR(logger_, "nullptr updater was specified"); + RCLCPP_ERROR(LOGGER, "nullptr updater was specified"); } void OccupancyMapMonitor::publishDebugInformation(bool flag) @@ -260,7 +262,7 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s rclcpp::Clock steady_clock(RCL_STEADY_TIME); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(logger_, steady_clock, 1000, "Incorrect mapping of mesh handles"); + RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000, "Incorrect mapping of mesh handles"); #pragma GCC diagnostic pop return false; } @@ -280,7 +282,7 @@ bool OccupancyMapMonitor::saveMapCallback(const std::shared_ptr& request, const std::shared_ptr& response) { - RCLCPP_INFO(logger_, "Writing map to %s", request->filename.c_str()); + RCLCPP_INFO(LOGGER, "Writing map to %s", request->filename.c_str()); tree_->lockRead(); try { @@ -298,7 +300,7 @@ bool OccupancyMapMonitor::loadMapCallback(const std::shared_ptr& request, const std::shared_ptr& response) { - RCLCPP_INFO(logger_, "Reading map from %s", request->filename.c_str()); + RCLCPP_INFO(LOGGER, "Reading map from %s", request->filename.c_str()); /* load the octree from disk */ tree_->lockWrite(); @@ -308,7 +310,7 @@ bool OccupancyMapMonitor::loadMapCallback(const std::shared_ptrsuccess = false; } tree_->unlockWrite(); 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..6f61c86fa1 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 @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -49,13 +48,15 @@ namespace occupancy_map_monitor { +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_monitor.middleware_handle"); +} 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, {} } { try { @@ -64,7 +65,7 @@ OccupancyMapMonitorMiddlewareHandle::OccupancyMapMonitorMiddlewareHandle(const r } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL_STREAM(logger_, "Exception while creating octomap updater plugin loader " << ex.what()); + RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating octomap updater plugin loader " << ex.what()); throw ex; } @@ -73,7 +74,7 @@ OccupancyMapMonitorMiddlewareHandle::OccupancyMapMonitorMiddlewareHandle(const r if (!node_->get_parameter("octomap_resolution", parameters_.map_resolution)) { parameters_.map_resolution = 0.1; - RCLCPP_WARN(logger_, "Resolution not specified for Octomap. Assuming resolution = %g instead", + RCLCPP_WARN(LOGGER, "Resolution not specified for Octomap. Assuming resolution = %g instead", parameters_.map_resolution); } } @@ -83,18 +84,18 @@ OccupancyMapMonitorMiddlewareHandle::OccupancyMapMonitorMiddlewareHandle(const r node_->get_parameter("octomap_frame", parameters_.map_frame); if (parameters_.map_frame.empty()) { - RCLCPP_ERROR(logger_, "No 'octomap_frame' parameter defined for octomap updates"); + RCLCPP_ERROR(LOGGER, "No 'octomap_frame' parameter defined for octomap updates"); } } std::vector sensor_names; if (!node_->get_parameter("sensors", sensor_names)) { - RCLCPP_ERROR(logger_, "No 3D sensor plugin(s) defined for octomap updates"); + RCLCPP_ERROR(LOGGER, "No 3D sensor plugin(s) defined for octomap updates"); } else if (sensor_names.empty()) { - RCLCPP_ERROR(logger_, "List of sensors is empty!"); + RCLCPP_ERROR(LOGGER, "List of sensors is empty!"); } for (const auto& sensor_name : sensor_names) @@ -102,12 +103,12 @@ OccupancyMapMonitorMiddlewareHandle::OccupancyMapMonitorMiddlewareHandle(const r std::string sensor_plugin = ""; if (!node_->get_parameter(sensor_name + ".sensor_plugin", sensor_plugin)) { - RCLCPP_ERROR(logger_, "No sensor plugin specified for octomap updater %s; ignoring.", sensor_name.c_str()); + RCLCPP_ERROR(LOGGER, "No sensor plugin specified for octomap updater %s; ignoring.", sensor_name.c_str()); } if (sensor_plugin.empty() || sensor_plugin[0] == '~') { - RCLCPP_INFO_STREAM(logger_, "Skipping octomap updater plugin '" << sensor_plugin << '\''); + RCLCPP_INFO_STREAM(LOGGER, "Skipping octomap updater plugin '" << sensor_plugin << '\''); continue; } else @@ -130,8 +131,8 @@ OccupancyMapUpdaterPtr OccupancyMapMonitorMiddlewareHandle::loadOccupancyMapUpda } catch (pluginlib::PluginlibException& exception) { - RCLCPP_ERROR_STREAM(logger_, "Exception while loading octomap updater '" << sensor_plugin - << "': " << exception.what() << '\n'); + RCLCPP_ERROR_STREAM(LOGGER, "Exception while loading octomap updater '" << sensor_plugin + << "': " << exception.what() << '\n'); } return nullptr; } @@ -140,7 +141,7 @@ void OccupancyMapMonitorMiddlewareHandle::initializeOccupancyMapUpdater(Occupanc { if (!occupancy_map_updater->initialize(node_)) { - RCLCPP_ERROR(logger_, "Unable to initialize map updater of type %s", occupancy_map_updater->getType().c_str()); + RCLCPP_ERROR(LOGGER, "Unable to initialize map updater of type %s", occupancy_map_updater->getType().c_str()); } } 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..2a0ee44842 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -46,7 +46,6 @@ #include #include #include -#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #else @@ -57,6 +56,8 @@ #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_server"); + static void publishOctomap(const rclcpp::Publisher::SharedPtr& octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor& server) { @@ -73,7 +74,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 +82,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(); 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..72b7c12e89 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp @@ -40,10 +40,10 @@ #include #include #include -#include namespace occupancy_map_monitor { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_updater"); OccupancyMapUpdater::OccupancyMapUpdater(const std::string& type) : type_(type) { @@ -89,7 +89,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, + LOGGER, 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 +101,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(LOGGER, 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/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index d54b7f475a..5f75d59eb0 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -106,6 +106,5 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater double inv_fx_, inv_fy_, K0_, K2_, K4_, K5_; std::vector filtered_labels_; rclcpp::Time last_depth_callback_start_; - rclcpp::Logger logger_; }; } // namespace occupancy_map_monitor 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..1cc05aac20 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 @@ -43,12 +43,12 @@ #include #include #include -#include #include namespace occupancy_map_monitor { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.depth_image_octomap_updater"); DepthImageOctomapUpdater::DepthImageOctomapUpdater() : OccupancyMapUpdater("DepthImageUpdater") @@ -71,7 +71,6 @@ DepthImageOctomapUpdater::DepthImageOctomapUpdater() , K2_(0.0) , K4_(0.0) , K5_(0.0) - , logger_(moveit::makeChildLogger("depth_image_octomap_updater")) { } @@ -100,7 +99,7 @@ bool DepthImageOctomapUpdater::setParams(const std::string& name_space) } catch (const rclcpp::exceptions::InvalidParameterTypeException& e) { - RCLCPP_ERROR_STREAM(logger_, e.what() << '\n'); + RCLCPP_ERROR_STREAM(LOGGER, e.what() << '\n'); return false; } } @@ -180,7 +179,7 @@ mesh_filter::MeshHandle DepthImageOctomapUpdater::excludeShape(const shapes::Sha } } else - RCLCPP_ERROR(logger_, "Mesh filter not yet initialized!"); + RCLCPP_ERROR(LOGGER, "Mesh filter not yet initialized!"); return h; } @@ -195,7 +194,7 @@ bool DepthImageOctomapUpdater::getShapeTransform(mesh_filter::MeshHandle h, Eige ShapeTransformCache::const_iterator it = transform_cache_.find(h); if (it == transform_cache_.end()) { - RCLCPP_ERROR(logger_, "Internal error. Mesh filter handle %u not found", h); + RCLCPP_ERROR(LOGGER, "Internal error. Mesh filter handle %u not found", h); return false; } transform = it->second; @@ -217,7 +216,7 @@ const bool HOST_IS_BIG_ENDIAN = []() { void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) { - RCLCPP_DEBUG(logger_, "Received a new depth image message (frame = '%s', encoding='%s')", + RCLCPP_DEBUG(LOGGER, "Received a new depth image message (frame = '%s', encoding='%s')", depth_msg->header.frame_id.c_str(), depth_msg->encoding.c_str()); rclcpp::Time start = node_->now(); @@ -310,7 +309,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 1000, + RCLCPP_WARN_THROTTLE(LOGGER, *node_->get_clock(), 1000, "More than half of the image messages discarded due to TF being unavailable (%u%%). " "Transform error of sensor data: %s; quitting callback.", (100 * failed_tf_) / (good_tf_ + failed_tf_), err.c_str()); @@ -320,7 +319,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_DEBUG_THROTTLE(logger_, *node_->get_clock(), 1000, + RCLCPP_DEBUG_THROTTLE(LOGGER, *node_->get_clock(), 1000, "Transform error of sensor data: %s; quitting callback", err.c_str()); #pragma GCC diagnostic pop } @@ -344,7 +343,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(logger_, *node_->get_clock(), 1000, "endian problem: received image data does not match host"); + RCLCPP_ERROR_THROTTLE(LOGGER, *node_->get_clock(), 1000, "endian problem: received image data does not match host"); #pragma GCC diagnostic pop } @@ -367,7 +366,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(logger_, *node_->get_clock(), 1000, "Unexpected encoding type: '%s'. Ignoring input.", + RCLCPP_ERROR_THROTTLE(LOGGER, *node_->get_clock(), 1000, "Unexpected encoding type: '%s'. Ignoring input.", depth_msg->encoding.c_str()); #pragma GCC diagnostic pop return; @@ -565,7 +564,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: catch (...) { tree_->unlockRead(); - RCLCPP_ERROR(logger_, "Internal error while parsing depth data"); + RCLCPP_ERROR(LOGGER, "Internal error while parsing depth data"); return; } tree_->unlockRead(); @@ -584,7 +583,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: } catch (...) { - RCLCPP_ERROR(logger_, "Internal error while updating octree"); + RCLCPP_ERROR(LOGGER, "Internal error while updating octree"); } tree_->unlockWrite(); tree_->triggerUpdateCallback(); @@ -592,6 +591,6 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: // at this point we still have not freed the space free_space_updater_->pushLazyUpdate(occupied_cells_ptr, model_cells_ptr, sensor_origin); - RCLCPP_DEBUG(logger_, "Processed depth image in %lf ms", (node_->now() - start).seconds() * 1000.0); + RCLCPP_DEBUG(LOGGER, "Processed depth image in %lf ms", (node_->now() - start).seconds() * 1000.0); } } // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h index cea8f3064b..c2686a8a02 100644 --- a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h +++ b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h @@ -41,7 +41,6 @@ #include #include #include -#include namespace occupancy_map_monitor { @@ -88,7 +87,5 @@ class LazyFreeSpaceUpdater std::thread update_thread_; std::thread process_thread_; - - rclcpp::Logger logger_; }; } // namespace occupancy_map_monitor 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..0ef67b8540 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 @@ -37,10 +37,10 @@ #include #include #include -#include namespace occupancy_map_monitor { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.lazy_free_space_updater"); LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const collision_detection::OccMapTreePtr& tree, unsigned int max_batch_size) : tree_(tree) @@ -51,7 +51,6 @@ 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")) { } @@ -73,7 +72,7 @@ LazyFreeSpaceUpdater::~LazyFreeSpaceUpdater() void LazyFreeSpaceUpdater::pushLazyUpdate(octomap::KeySet* occupied_cells, octomap::KeySet* model_cells, const octomap::point3d& sensor_origin) { - RCLCPP_DEBUG(logger_, "Pushing %lu occupied cells and %lu model cells for lazy updating...", + RCLCPP_DEBUG(LOGGER, "Pushing %lu occupied cells and %lu model cells for lazy updating...", static_cast(occupied_cells->size()), static_cast(model_cells->size())); std::scoped_lock _(update_cell_sets_lock_); @@ -99,7 +98,7 @@ void LazyFreeSpaceUpdater::pushBatchToProcess(OcTreeKeyCountMap* occupied_cells, } else { - RCLCPP_WARN(logger_, "Previous batch update did not complete. Ignoring set of cells to be freed."); + RCLCPP_WARN(LOGGER, "Previous batch update did not complete. Ignoring set of cells to be freed."); delete occupied_cells; delete model_cells; } @@ -125,7 +124,7 @@ void LazyFreeSpaceUpdater::processThread() if (!running_) break; - RCLCPP_DEBUG(logger_, + RCLCPP_DEBUG(LOGGER, "Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", static_cast(process_occupied_cells_set_->size()), static_cast(process_model_cells_set_->size())); @@ -176,7 +175,7 @@ void LazyFreeSpaceUpdater::processThread() free_cells1.erase(it); free_cells2.erase(it); } - RCLCPP_DEBUG(logger_, "Marking %lu cells as free...", + RCLCPP_DEBUG(LOGGER, "Marking %lu cells as free...", static_cast(free_cells1.size() + free_cells2.size())); tree_->lockWrite(); @@ -195,12 +194,12 @@ void LazyFreeSpaceUpdater::processThread() } catch (...) { - RCLCPP_ERROR(logger_, "Internal error while updating octree"); + RCLCPP_ERROR(LOGGER, "Internal error while updating octree"); } tree_->unlockWrite(); tree_->triggerUpdateCallback(); - RCLCPP_DEBUG(logger_, "Marked free cells in %lf ms", (clock.now() - start).seconds() * 1000.0); + RCLCPP_DEBUG(LOGGER, "Marked free cells in %lf ms", (clock.now() - start).seconds() * 1000.0); delete process_occupied_cells_set_; process_occupied_cells_set_ = nullptr; @@ -244,7 +243,7 @@ void LazyFreeSpaceUpdater::lazyUpdateThread() { if ((sensor_origins_.front() - sensor_origin).norm() > max_sensor_delta_) { - RCLCPP_DEBUG(logger_, "Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", + RCLCPP_DEBUG(LOGGER, "Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", batch_size); pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin); batch_size = 0; @@ -266,7 +265,7 @@ void LazyFreeSpaceUpdater::lazyUpdateThread() if (batch_size >= max_batch_size_) { - RCLCPP_DEBUG(logger_, "Pushing %u sets of occupied/model cells to free cells update thread", batch_size); + RCLCPP_DEBUG(LOGGER, "Pushing %u sets of occupied/model cells to free cells update thread", batch_size); pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin); occupied_cells_set = nullptr; batch_size = 0; diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 0f7a694e54..543b341920 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -43,7 +43,6 @@ #endif #include #include -#include #include #include #include @@ -54,6 +53,8 @@ using namespace std; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.gl_renderer"); + mesh_filter::GLRenderer::GLRenderer(unsigned width, unsigned height, double near, double far) : width_(width) , height_(height) @@ -344,7 +345,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(LOGGER, "%s\n", &program_error_message[0]); } if (vertex_shader_id) diff --git a/moveit_ros/perception/point_containment_filter/CMakeLists.txt b/moveit_ros/perception/point_containment_filter/CMakeLists.txt index 44f72bd3f1..fafbf8a594 100644 --- a/moveit_ros/perception/point_containment_filter/CMakeLists.txt +++ b/moveit_ros/perception/point_containment_filter/CMakeLists.txt @@ -6,7 +6,6 @@ ament_target_dependencies(moveit_point_containment_filter rclcpp sensor_msgs geometric_shapes - moveit_core ) install(DIRECTORY include/ DESTINATION include/moveit_ros_perception) 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..55ac24d551 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -39,7 +39,8 @@ #include #include #include -#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.shape_mask"); point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) : transform_callback_(transform_callback), next_handle_(1), min_handle_(1) @@ -80,10 +81,7 @@ point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addSh ss.handle = next_handle_; 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(LOGGER, "Internal error in management of bodies in ShapeMask. This is a serious error."); used_handles_[next_handle_] = insert_op.first; } else @@ -116,7 +114,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(LOGGER, "Unable to remove shape handle %u", handle); } void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::msg::PointCloud2& data_in, @@ -143,12 +141,11 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::msg { if (!it->body) { - RCLCPP_ERROR_STREAM(moveit::getLogger(), - "Missing transform for shape with handle " << it->handle << " without a body"); + RCLCPP_ERROR_STREAM(LOGGER, "Missing transform for shape with handle " << it->handle << " without a body"); } else { - RCLCPP_ERROR_STREAM(moveit::getLogger(), + RCLCPP_ERROR_STREAM(LOGGER, "Missing transform for shape " << it->body->getType() << " with handle " << it->handle); } } diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 6f734936c1..57512f959e 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -101,7 +101,5 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater std::unique_ptr shape_mask_; std::vector mask_; - - rclcpp::Logger logger_; }; } // namespace occupancy_map_monitor 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..2e34d4df8f 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 @@ -43,12 +43,12 @@ #include #include #include -#include #include namespace occupancy_map_monitor { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.pointcloud_octomap_updater"); PointCloudOctomapUpdater::PointCloudOctomapUpdater() : OccupancyMapUpdater("PointCloudUpdater") , scale_(1.0) @@ -58,7 +58,6 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater() , max_update_rate_(0) , point_cloud_subscriber_(nullptr) , point_cloud_filter_(nullptr) - , logger_(moveit::makeChildLogger("pointcloud_octomap_updater")) { } @@ -114,14 +113,14 @@ void PointCloudOctomapUpdater::start() *point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, node_); point_cloud_filter_->registerCallback( [this](const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud) { cloudMsgCallback(cloud); }); - RCLCPP_INFO(logger_, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), + RCLCPP_INFO(LOGGER, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str()); } else { point_cloud_subscriber_->registerCallback( [this](const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud) { cloudMsgCallback(cloud); }); - RCLCPP_INFO(logger_, "Listening to '%s'", point_cloud_topic_.c_str()); + RCLCPP_INFO(LOGGER, "Listening to '%s'", point_cloud_topic_.c_str()); } } @@ -142,7 +141,7 @@ ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr& } else { - RCLCPP_ERROR(logger_, "Shape filter not yet initialized!"); + RCLCPP_ERROR(LOGGER, "Shape filter not yet initialized!"); } return h; } @@ -170,7 +169,7 @@ void PointCloudOctomapUpdater::updateMask(const sensor_msgs::msg::PointCloud2& / void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud_msg) { - RCLCPP_DEBUG(logger_, "Received a new point cloud message"); + RCLCPP_DEBUG(LOGGER, "Received a new point cloud message"); rclcpp::Time start = rclcpp::Clock(RCL_ROS_TIME).now(); if (max_update_rate_ > 0) @@ -202,7 +201,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::msg::PointClo } catch (tf2::TransformException& ex) { - RCLCPP_ERROR_STREAM(logger_, "Transform error of sensor data: " << ex.what() << "; quitting callback"); + RCLCPP_ERROR_STREAM(LOGGER, "Transform error of sensor data: " << ex.what() << "; quitting callback"); return; } } @@ -359,10 +358,10 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::msg::PointClo } catch (...) { - RCLCPP_ERROR(logger_, "Internal error while updating octree"); + RCLCPP_ERROR(LOGGER, "Internal error while updating octree"); } tree_->unlockWrite(); - RCLCPP_DEBUG(logger_, "Processed point cloud in %lf ms", (node_->now() - start).seconds() * 1000.0); + RCLCPP_DEBUG(LOGGER, "Processed point cloud in %lf ms", (node_->now() - start).seconds() * 1000.0); tree_->triggerUpdateCallback(); if (filtered_cloud) diff --git a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h index 1f56067e12..bfc316aa18 100644 --- a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h +++ b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h @@ -158,8 +158,6 @@ class SemanticWorld TableCallbackFn table_callback_; rclcpp::Publisher::SharedPtr planning_scene_diff_publisher_; - - rclcpp::Logger logger_; }; } // namespace semantic_world } // namespace moveit diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 9eba619e43..e63ac1c5f2 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -40,7 +40,6 @@ #include #include #include -#include // OpenCV #include #include @@ -64,10 +63,11 @@ namespace moveit { namespace semantic_world { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.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) { table_subscriber_ = node_handle_->create_subscription( @@ -83,7 +83,7 @@ SemanticWorld::SemanticWorld(const rclcpp::Node::SharedPtr& node, visualization_msgs::msg::MarkerArray SemanticWorld::getPlaceLocationsMarker(const std::vector& poses) const { - RCLCPP_DEBUG(logger_, "Visualizing: %d place poses", static_cast(poses.size())); + RCLCPP_DEBUG(LOGGER, "Visualizing: %d place poses", static_cast(poses.size())); visualization_msgs::msg::MarkerArray marker; for (std::size_t i = 0; i < poses.size(); ++i) { @@ -241,7 +241,7 @@ SemanticWorld::generatePlacePoses(const std::string& table_name, const shapes::S } std::vector place_poses; - RCLCPP_ERROR(logger_, "Did not find table %s to place on", table_name.c_str()); + RCLCPP_ERROR(LOGGER, "Did not find table %s to place on", table_name.c_str()); return place_poses; } @@ -372,7 +372,7 @@ SemanticWorld::generatePlacePoses(const object_recognition_msgs::msg::Table& tab unsigned int num_x = fabs(x_max - x_min) / resolution + 1; unsigned int num_y = fabs(y_max - y_min) / resolution + 1; - RCLCPP_DEBUG(logger_, "Num points for possible place operations: %d %d", num_x, num_y); + RCLCPP_DEBUG(LOGGER, "Num points for possible place operations: %d %d", num_x, num_y); std::vector > contours; std::vector hierarchy; @@ -469,7 +469,7 @@ bool SemanticWorld::isInsideTableContour(const geometry_msgs::msg::Pose& pose, // Assuming Z axis points upwards for the table if (point.z() < -fabs(min_vertical_offset)) { - RCLCPP_ERROR(logger_, "Object is not above table"); + RCLCPP_ERROR(LOGGER, "Object is not above table"); return false; } @@ -477,7 +477,7 @@ bool SemanticWorld::isInsideTableContour(const geometry_msgs::msg::Pose& pose, int point_y = (point.y() - y_min) * scale_factor; cv::Point2f point2f(point_x, point_y); double result = cv::pointPolygonTest(contours[0], point2f, true); - RCLCPP_DEBUG(logger_, "table distance: %f", result); + RCLCPP_DEBUG(LOGGER, "table distance: %f", result); return static_cast(result) >= static_cast(min_distance_from_edge * scale_factor); } @@ -488,7 +488,7 @@ std::string SemanticWorld::findObjectTable(const geometry_msgs::msg::Pose& pose, std::map::const_iterator it; for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it) { - RCLCPP_DEBUG_STREAM(logger_, "Testing table: " << it->first); + RCLCPP_DEBUG_STREAM(LOGGER, "Testing table: " << it->first); if (isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset)) return it->first; } @@ -498,12 +498,12 @@ std::string SemanticWorld::findObjectTable(const geometry_msgs::msg::Pose& pose, void SemanticWorld::tableCallback(const object_recognition_msgs::msg::TableArray::ConstSharedPtr& msg) { table_array_ = *msg; - RCLCPP_INFO(logger_, "Table callback with %d tables", static_cast(table_array_.tables.size())); + RCLCPP_INFO(LOGGER, "Table callback with %d tables", static_cast(table_array_.tables.size())); transformTableArray(table_array_); // Callback on an update if (table_callback_) { - RCLCPP_INFO(logger_, "Calling table callback"); + RCLCPP_INFO(LOGGER, "Calling table callback"); table_callback_(); } } @@ -515,8 +515,8 @@ void SemanticWorld::transformTableArray(object_recognition_msgs::msg::TableArray std::string original_frame = table.header.frame_id; if (table.convex_hull.empty()) continue; - RCLCPP_INFO_STREAM(logger_, "Original pose: " << table.pose.position.x << ',' << table.pose.position.y << ',' - << table.pose.position.z); + RCLCPP_INFO_STREAM(LOGGER, "Original pose: " << table.pose.position.x << ',' << table.pose.position.y << ',' + << table.pose.position.z); std::string error_text; const Eigen::Isometry3d& original_transform = planning_scene_->getFrameTransform(original_frame); Eigen::Isometry3d original_pose; @@ -524,10 +524,10 @@ void SemanticWorld::transformTableArray(object_recognition_msgs::msg::TableArray original_pose = original_transform * original_pose; table.pose = tf2::toMsg(original_pose); table.header.frame_id = planning_scene_->getTransforms().getTargetFrame(); - RCLCPP_INFO_STREAM(logger_, "Successfully transformed table array from " << original_frame << "to " - << table.header.frame_id); - RCLCPP_INFO_STREAM(logger_, "Transformed pose: " << table.pose.position.x << ',' << table.pose.position.y << ',' - << table.pose.position.z); + RCLCPP_INFO_STREAM(LOGGER, "Successfully transformed table array from " << original_frame << "to " + << table.header.frame_id); + RCLCPP_INFO_STREAM(LOGGER, "Transformed pose: " << table.pose.position.x << ',' << table.pose.position.y << ',' + << table.pose.position.z); } } diff --git a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h index 93c67a8d5d..e3b1958dd5 100644 --- a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h +++ b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h @@ -43,13 +43,8 @@ namespace collision_detection class CollisionPluginLoader : public CollisionPluginCache { public: - CollisionPluginLoader(); - /** @brief Fetch plugin name from parameter server and activate the plugin for the given scene */ void setupScene(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene); - -private: - rclcpp::Logger logger_; }; } // namespace collision_detection 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..dd62628b36 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 @@ -33,22 +33,18 @@ *********************************************************************/ #include -#include +static const std::string LOGNAME = "collision_detection"; namespace collision_detection { static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_plugin_loader"); -CollisionPluginLoader::CollisionPluginLoader() : logger_(moveit::makeChildLogger("collision_plugin_loader")) -{ -} - void CollisionPluginLoader::setupScene(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene) { if (!scene) { - RCLCPP_WARN(logger_, "Cannot setup scene, PlanningScenePtr is null."); + RCLCPP_WARN(LOGGER, "Cannot setup scene, PlanningScenePtr is null."); return; } @@ -77,7 +73,7 @@ void CollisionPluginLoader::setupScene(const rclcpp::Node::SharedPtr& node, } activate(collision_detector_name, scene); - RCLCPP_INFO(logger_, "Using collision detector: %s", scene->getCollisionDetectorName().c_str()); + RCLCPP_INFO(LOGGER, "Using collision detector: %s", scene->getCollisionDetectorName().c_str()); } } // namespace collision_detection 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..b49c616cd2 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 @@ -41,16 +41,15 @@ #include #include #include -#include namespace constraint_sampler_manager_loader { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.constraint_sampler_manager_loader"); class ConstraintSamplerManagerLoader::Helper { public: - Helper(const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) - : node_(node), logger_(moveit::makeChildLogger("constraint_sampler_manager_loader")) + Helper(const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) : node_(node) { if (node_->has_parameter("constraint_samplers")) { @@ -64,7 +63,7 @@ class ConstraintSamplerManagerLoader::Helper } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(logger_, "Exception while creating constraint sampling plugin loader %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while creating constraint sampling plugin loader %s", ex.what()); return; } boost::char_separator sep(" "); @@ -76,12 +75,11 @@ class ConstraintSamplerManagerLoader::Helper constraint_samplers::ConstraintSamplerAllocatorPtr csa = constraint_sampler_plugin_loader_->createUniqueInstance(*beg); csm->registerSamplerAllocator(csa); - RCLCPP_INFO(logger_, "Loaded constraint sampling plugin %s", std::string(*beg).c_str()); + RCLCPP_INFO(LOGGER, "Loaded constraint sampling plugin %s", std::string(*beg).c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(logger_, "Exception while planning adapter plugin '%s': %s", std::string(*beg).c_str(), - ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while planning adapter plugin '%s': %s", std::string(*beg).c_str(), ex.what()); } } } @@ -91,7 +89,6 @@ class ConstraintSamplerManagerLoader::Helper const rclcpp::Node::SharedPtr node_; std::unique_ptr> constraint_sampler_plugin_loader_; - rclcpp::Logger logger_; }; ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader( const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) diff --git a/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp index 274de0ebe4..30a1793966 100644 --- a/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp +++ b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp @@ -42,14 +42,12 @@ #include #include #include -#include int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared("list_planning_adapter_plugins"); - moveit::setLogger(node->get_logger()); std::unique_ptr> loader; try 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..af4f6c8807 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 @@ -40,7 +40,6 @@ #include #include #include -#include namespace kinematics_plugin_loader { @@ -57,7 +56,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) { } @@ -90,6 +89,5 @@ class KinematicsPluginLoader std::vector groups_; std::map ik_timeout_; - rclcpp::Logger logger_; }; } // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index db723955d5..0546581165 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -46,10 +46,10 @@ #include #include #include -#include namespace kinematics_plugin_loader { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_plugin_loader"); class KinematicsPluginLoader::KinematicsLoaderImpl { public: @@ -61,12 +61,11 @@ class KinematicsPluginLoader::KinematicsLoaderImpl */ KinematicsLoaderImpl(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, const std::map& possible_kinematics_solvers, - const std::map& search_res, const rclcpp::Logger& logger) + const std::map& search_res) : node_(node) , robot_description_(robot_description) , possible_kinematics_solvers_(possible_kinematics_solvers) , search_res_(search_res) - , logger_(logger) { try { @@ -76,7 +75,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl } catch (pluginlib::PluginlibException& e) { - RCLCPP_ERROR(logger_, "Unable to construct kinematics loader. Error: %s", e.what()); + RCLCPP_ERROR(LOGGER, "Unable to construct kinematics loader. Error: %s", e.what()); } } @@ -89,7 +88,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl { std::vector tips; // get the last link in the chain - RCLCPP_DEBUG(logger_, + RCLCPP_DEBUG(LOGGER, "Choosing tip frame of kinematic solver for group %s" "based on last link in chain", jmg->getName().c_str()); @@ -99,7 +98,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl // Error check if (tips.empty()) { - RCLCPP_ERROR(logger_, "Error choosing kinematic solver tip frame(s)."); + RCLCPP_ERROR(LOGGER, "Error choosing kinematic solver tip frame(s)."); } // Debug tip choices @@ -107,7 +106,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl tip_debug << "Planning group '" << jmg->getName() << "' has tip(s): "; for (const auto& tip : tips) tip_debug << tip << ", "; - RCLCPP_DEBUG_STREAM(logger_, tip_debug.str()); + RCLCPP_DEBUG_STREAM(LOGGER, tip_debug.str()); return tips; } @@ -117,23 +116,23 @@ class KinematicsPluginLoader::KinematicsLoaderImpl kinematics::KinematicsBasePtr result; if (!kinematics_loader_) { - RCLCPP_ERROR(logger_, "Invalid kinematics loader."); + RCLCPP_ERROR(LOGGER, "Invalid kinematics loader."); return result; } if (!jmg) { - RCLCPP_ERROR(logger_, "Specified group is nullptr. Cannot allocate kinematics solver."); + RCLCPP_ERROR(LOGGER, "Specified group is nullptr. Cannot allocate kinematics solver."); return result; } const std::vector& links = jmg->getLinkModels(); if (links.empty()) { - RCLCPP_ERROR(logger_, "No links specified for group '%s'. Cannot allocate kinematics solver.", + RCLCPP_ERROR(LOGGER, "No links specified for group '%s'. Cannot allocate kinematics solver.", jmg->getName().c_str()); return result; } - RCLCPP_DEBUG(logger_, "Trying to allocate kinematics solver for group '%s'", jmg->getName().c_str()); + RCLCPP_DEBUG(LOGGER, "Trying to allocate kinematics solver for group '%s'", jmg->getName().c_str()); const std::string& base = links.front()->getParentJointModel()->getParentLinkModel() ? links.front()->getParentJointModel()->getParentLinkModel()->getName() : @@ -162,14 +161,14 @@ class KinematicsPluginLoader::KinematicsLoaderImpl if (!result->initialize(node_, jmg->getParentModel(), jmg->getName(), (base.empty() || base[0] != '/') ? base : base.substr(1), tips, search_res)) { - RCLCPP_ERROR(logger_, "Kinematics solver of type '%s' could not be initialized for group '%s'", + RCLCPP_ERROR(LOGGER, "Kinematics solver of type '%s' could not be initialized for group '%s'", solver.c_str(), jmg->getName().c_str()); result.reset(); continue; } result->setDefaultTimeout(jmg->getDefaultIKTimeout()); - RCLCPP_DEBUG(logger_, + RCLCPP_DEBUG(LOGGER, "Successfully allocated and initialized a kinematics solver of type '%s' with search " "resolution %lf for group '%s' at address %p", solver.c_str(), search_res, jmg->getName().c_str(), result.get()); @@ -178,14 +177,14 @@ class KinematicsPluginLoader::KinematicsLoaderImpl } catch (pluginlib::PluginlibException& e) { - RCLCPP_ERROR(logger_, "The kinematics plugin (%s) failed to load. Error: %s", solver.c_str(), e.what()); + RCLCPP_ERROR(LOGGER, "The kinematics plugin (%s) failed to load. Error: %s", solver.c_str(), e.what()); } } if (!result) { - RCLCPP_DEBUG(logger_, "No usable kinematics solver was found for this group.\n" - "Did you load kinematics.yaml into your node's namespace?"); + RCLCPP_DEBUG(LOGGER, "No usable kinematics solver was found for this group.\n" + "Did you load kinematics.yaml into your node's namespace?"); } return result; } @@ -209,7 +208,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl { for (const auto& [group, solver] : possible_kinematics_solvers_) { - RCLCPP_INFO(logger_, "Solver for group '%s': '%s' (search resolution = %lf)", group.c_str(), solver.c_str(), + RCLCPP_INFO(LOGGER, "Solver for group '%s': '%s' (search resolution = %lf)", group.c_str(), solver.c_str(), search_res_.at(group)); } } @@ -223,7 +222,6 @@ class KinematicsPluginLoader::KinematicsLoaderImpl std::map instances_; std::mutex lock_; std::mutex cache_lock_; - rclcpp::Logger logger_; }; void KinematicsPluginLoader::status() const @@ -234,7 +232,7 @@ void KinematicsPluginLoader::status() const } else { - RCLCPP_INFO(logger_, "Loader function was never required"); + RCLCPP_INFO(LOGGER, "Loader function was never required"); } } @@ -242,7 +240,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const { if (!loader_) { - RCLCPP_DEBUG(logger_, "Configuring kinematics solvers"); + RCLCPP_DEBUG(LOGGER, "Configuring kinematics solvers"); groups_.clear(); std::map possible_kinematics_solvers; @@ -253,7 +251,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const { const std::vector& known_groups = srdf_model->getGroups(); - RCLCPP_DEBUG(logger_, "Loading kinematics parameters..."); + RCLCPP_DEBUG(LOGGER, "Loading kinematics parameters..."); // read the list of plugin names for possible kinematics solvers for (const srdf::Model::Group& known_group : known_groups) { @@ -267,7 +265,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const if (kinematics_solver.empty()) { - RCLCPP_DEBUG(logger_, "No kinematics solver specified for group '%s'.", known_group.name_.c_str()); + RCLCPP_DEBUG(LOGGER, "No kinematics solver specified for group '%s'.", known_group.name_.c_str()); continue; } @@ -275,26 +273,26 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const groups_.push_back(known_group.name_); possible_kinematics_solvers[known_group.name_] = kinematics_solver; - RCLCPP_DEBUG(logger_, "Found kinematics solver '%s' for group '%s'.", kinematics_solver.c_str(), + RCLCPP_DEBUG(LOGGER, "Found kinematics solver '%s' for group '%s'.", kinematics_solver.c_str(), known_group.name_.c_str()); std::string kinematics_solver_res_param_name = kinematics_param_prefix + ".kinematics_solver_search_resolution"; const auto kinematics_solver_search_resolution = group_params_.at(known_group.name_).kinematics_solver_search_resolution; search_res[known_group.name_] = kinematics_solver_search_resolution; - RCLCPP_DEBUG(logger_, "Found param %s : %f", kinematics_solver_res_param_name.c_str(), + RCLCPP_DEBUG(LOGGER, "Found param %s : %f", kinematics_solver_res_param_name.c_str(), kinematics_solver_search_resolution); std::string kinematics_solver_timeout_param_name = kinematics_param_prefix + ".kinematics_solver_timeout"; const auto kinematics_solver_timeout = group_params_.at(known_group.name_).kinematics_solver_timeout; ik_timeout_[known_group.name_] = kinematics_solver_timeout; - RCLCPP_DEBUG(logger_, "Found param %s : %f", kinematics_solver_timeout_param_name.c_str(), + RCLCPP_DEBUG(LOGGER, "Found param %s : %f", kinematics_solver_timeout_param_name.c_str(), kinematics_solver_timeout); } } - loader_ = std::make_shared(node_, robot_description_, possible_kinematics_solvers, search_res, - logger_); + loader_ = + std::make_shared(node_, robot_description_, possible_kinematics_solvers, search_res); } return [&loader = *loader_](const moveit::core::JointModelGroup* jmg) { diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index f3c6edaa7b..a9d6fe3f9d 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -183,8 +183,6 @@ class MoveItCpp // Execution trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; - rclcpp::Logger logger_; - /** \brief Initialize and setup the planning scene monitor */ bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options); diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 88668e1f01..4d134810c0 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -239,7 +239,6 @@ class PlanningComponent moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_; bool workspace_parameters_set_ = false; - rclcpp::Logger logger_; // common properties for goals // TODO(henningkayser): support goal tolerances diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index d3c0bb967d..f8ef3b7fbd 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -41,22 +41,22 @@ #include #include #include -#include namespace moveit_cpp { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning_interface.moveit_cpp"); + MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node) : MoveItCpp(node, Options(node)) { } -MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options) - : node_(node), logger_(moveit::makeChildLogger("moveit_cpp")) +MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options) : node_(node) { // Configure planning scene monitor if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options)) { const std::string error = "Unable to configure planning scene monitor"; - RCLCPP_FATAL_STREAM(logger_, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } @@ -64,7 +64,7 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options { const std::string error = "Unable to construct robot model. Please make sure all needed information is on the " "parameter server."; - RCLCPP_FATAL_STREAM(logger_, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } @@ -72,19 +72,19 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options if (load_planning_pipelines && !loadPlanningPipelines(options.planning_pipeline_options)) { const std::string error = "Failed to load planning pipelines from parameter server"; - RCLCPP_FATAL_STREAM(logger_, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } trajectory_execution_manager_ = std::make_shared( node_, getRobotModel(), planning_scene_monitor_->getStateMonitor()); - RCLCPP_DEBUG(logger_, "MoveItCpp running"); + RCLCPP_DEBUG(LOGGER, "MoveItCpp running"); } MoveItCpp::~MoveItCpp() { - RCLCPP_INFO(logger_, "Deleting MoveItCpp"); + RCLCPP_INFO(LOGGER, "Deleting MoveItCpp"); } bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options) @@ -92,11 +92,11 @@ bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opti planning_scene_monitor_ = std::make_shared(node_, options.robot_description, options.name); // Allows us to synchronize to Rviz and also publish collision objects to ourselves - RCLCPP_DEBUG(logger_, "Configuring Planning Scene Monitor"); + RCLCPP_DEBUG(LOGGER, "Configuring Planning Scene Monitor"); if (planning_scene_monitor_->getPlanningScene()) { // Start state and scene monitors - RCLCPP_INFO(logger_, "Listening to '%s' for joint states", options.joint_state_topic.c_str()); + RCLCPP_INFO(LOGGER, "Listening to '%s' for joint states", options.joint_state_topic.c_str()); // Subscribe to JointState sensor messages planning_scene_monitor_->startStateMonitor(options.joint_state_topic, options.attached_collision_object_topic); // Publish planning scene updates to remote monitors like RViz @@ -109,7 +109,7 @@ bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opti } else { - RCLCPP_ERROR(logger_, "Planning scene not configured"); + RCLCPP_ERROR(LOGGER, "Planning scene not configured"); return false; } @@ -131,7 +131,7 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options) if (planning_pipelines_.empty()) { - RCLCPP_ERROR(logger_, "Failed to load any planning pipelines."); + RCLCPP_ERROR(LOGGER, "Failed to load any planning pipelines."); return false; } return true; @@ -151,7 +151,7 @@ bool MoveItCpp::getCurrentState(moveit::core::RobotStatePtr& current_state, doub { if (wait_seconds > 0.0 && !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(node_->now(), wait_seconds)) { - RCLCPP_ERROR(logger_, "Did not receive robot state"); + RCLCPP_ERROR(LOGGER, "Did not receive robot state"); return false; } { // Lock planning scene @@ -206,7 +206,7 @@ MoveItCpp::execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, { if (!robot_trajectory) { - RCLCPP_ERROR(logger_, "Robot trajectory is undefined"); + RCLCPP_ERROR(LOGGER, "Robot trajectory is undefined"); return moveit_controller_manager::ExecutionStatus::ABORTED; } @@ -215,7 +215,7 @@ MoveItCpp::execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, // Check if there are controllers that can handle the execution if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name)) { - RCLCPP_ERROR(logger_, "Execution failed! No active controllers configured for group '%s'", group_name.c_str()); + RCLCPP_ERROR(LOGGER, "Execution failed! No active controllers configured for group '%s'", group_name.c_str()); return moveit_controller_manager::ExecutionStatus::ABORTED; } @@ -240,7 +240,7 @@ bool MoveItCpp::terminatePlanningPipeline(const std::string& pipeline_name) } catch (const std::out_of_range& oor) { - RCLCPP_ERROR(logger_, "Cannot terminate pipeline '%s' because no pipeline with that name exists", + RCLCPP_ERROR(LOGGER, "Cannot terminate pipeline '%s' because no pipeline with that name exists", pipeline_name.c_str()); return false; } diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index c3877e39f9..d161be7f83 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -42,22 +42,19 @@ #include #include #include -#include namespace moveit_cpp { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning_interface.planning_component"); PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp) - : node_(moveit_cpp->getNode()) - , moveit_cpp_(moveit_cpp) - , group_name_(group_name) - , logger_(moveit::makeChildLogger("planning_component")) + : node_(moveit_cpp->getNode()), moveit_cpp_(moveit_cpp), group_name_(group_name) { joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); if (!joint_model_group_) { std::string error = "Could not find joint model group '" + group_name + "'."; - RCLCPP_FATAL_STREAM(logger_, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } } @@ -69,7 +66,7 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const rclcpp if (!joint_model_group_) { std::string error = "Could not find joint model group '" + group_name + "'."; - RCLCPP_FATAL_STREAM(logger_, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } } @@ -82,7 +79,7 @@ const std::vector PlanningComponent::getNamedTargetStates() } else { - RCLCPP_WARN(logger_, "Unable to find joint group with name '%s'.", group_name_.c_str()); + RCLCPP_WARN(LOGGER, "Unable to find joint group with name '%s'.", group_name_.c_str()); } std::vector empty; @@ -114,7 +111,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest // check if joint_model_group exists if (!joint_model_group_) { - RCLCPP_ERROR(logger_, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; return plan_solution; } @@ -122,7 +119,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest // Check if goal constraints exist if (current_goal_constraints_.empty()) { - RCLCPP_ERROR(logger_, "No goal constraints set for planning request"); + RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return plan_solution; } @@ -159,7 +156,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan( // check if joint_model_group exists if (!joint_model_group_) { - RCLCPP_ERROR(logger_, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; return plan_solution; } @@ -167,7 +164,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan( // Check if goal constraints exist if (current_goal_constraints_.empty()) { - RCLCPP_ERROR(logger_, "No goal constraints set for planning request"); + RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return plan_solution; } @@ -204,7 +201,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan( } catch (std::out_of_range&) { - RCLCPP_ERROR(logger_, "MotionPlanResponse vector was empty after parallel planning"); + RCLCPP_ERROR(LOGGER, "MotionPlanResponse vector was empty after parallel planning"); plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; } // Run planning attempt @@ -216,13 +213,13 @@ planning_interface::MotionPlanResponse PlanningComponent::plan() PlanRequestParameters plan_request_parameters; plan_request_parameters.load(node_); RCLCPP_DEBUG_STREAM( - logger_, "Default plan request parameters loaded with --" - << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ',' - << " planner_id: " << plan_request_parameters.planner_id << ',' - << " planning_time: " << plan_request_parameters.planning_time << ',' - << " planning_attempts: " << plan_request_parameters.planning_attempts << ',' - << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ',' - << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); + LOGGER, "Default plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ',' + << " planner_id: " << plan_request_parameters.planner_id << ',' + << " planning_time: " << plan_request_parameters.planning_time << ',' + << " planning_attempts: " << plan_request_parameters.planning_attempts << ',' + << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ',' + << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); return plan(plan_request_parameters); } @@ -251,7 +248,7 @@ bool PlanningComponent::setStartState(const std::string& start_state_name) const auto& named_targets = getNamedTargetStates(); if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end()) { - RCLCPP_ERROR(logger_, "No predefined joint state found for target name '%s'", start_state_name.c_str()); + RCLCPP_ERROR(LOGGER, "No predefined joint state found for target name '%s'", start_state_name.c_str()); return false; } moveit::core::RobotState start_state(moveit_cpp_->getRobotModel()); @@ -314,7 +311,7 @@ bool PlanningComponent::setGoal(const std::string& goal_state_name) const auto& named_targets = getNamedTargetStates(); if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end()) { - RCLCPP_ERROR(logger_, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); + RCLCPP_ERROR(LOGGER, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); return false; } moveit::core::RobotState goal_state(moveit_cpp_->getRobotModel()); diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index 5f2d9dd80b..4d63f066b7 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -42,7 +42,6 @@ #include #include #include -#include #include @@ -183,8 +182,6 @@ class PlanExecution bool execution_complete_; bool path_became_invalid_; - rclcpp::Logger logger_; - // class DynamicReconfigureImpl; // DynamicReconfigureImpl* reconfigure_impl_; }; diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 5e0133e6ce..39c938d92d 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -47,13 +47,13 @@ #include #include #include -#include // #include // #include namespace plan_execution { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.plan_execution"); // class PlanExecution::DynamicReconfigureImpl // { @@ -80,10 +80,7 @@ namespace plan_execution plan_execution::PlanExecution::PlanExecution( const rclcpp::Node::SharedPtr& node, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution) - : node_(node) - , planning_scene_monitor_(planning_scene_monitor) - , trajectory_execution_manager_(trajectory_execution) - , logger_(moveit::makeChildLogger("add_time_optimal_parameterization")) + : node_(node), planning_scene_monitor_(planning_scene_monitor), trajectory_execution_manager_(trajectory_execution) { if (!trajectory_execution_manager_) { @@ -162,7 +159,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p do { replan_attempts++; - RCLCPP_INFO(logger_, "Planning attempt %u of at most %u", replan_attempts, max_replan_attempts); + RCLCPP_INFO(LOGGER, "Planning attempt %u of at most %u", replan_attempts, max_replan_attempts); if (opt.before_plan_callback_) opt.before_plan_callback_(); @@ -232,10 +229,10 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p // otherwise, we wait (if needed) if (opt.replan_delay > 0.0) { - RCLCPP_INFO(logger_, "Waiting for a %lf seconds before attempting a new plan ...", opt.replan_delay); + RCLCPP_INFO(LOGGER, "Waiting for a %lf seconds before attempting a new plan ...", opt.replan_delay); auto replan_delay_seconds = std::chrono::duration(opt.replan_delay); rclcpp::sleep_for(std::chrono::duration_cast(replan_delay_seconds)); - RCLCPP_INFO(logger_, "Done waiting"); + RCLCPP_INFO(node_->get_logger(), "Done waiting"); } } @@ -247,7 +244,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p if (preempt_requested) { - RCLCPP_DEBUG(logger_, "PlanExecution was preempted"); + RCLCPP_DEBUG(LOGGER, "PlanExecution was preempted"); plan.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED; } @@ -256,11 +253,11 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p if (plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - RCLCPP_DEBUG(logger_, "PlanExecution finished successfully."); + RCLCPP_DEBUG(LOGGER, "PlanExecution finished successfully."); } else { - RCLCPP_DEBUG(logger_, "PlanExecution terminating with error code %d - '%s'", plan.error_code.val, + RCLCPP_DEBUG(LOGGER, "PlanExecution terminating with error code %d - '%s'", plan.error_code.val, moveit::core::errorCodeToString(plan.error_code).c_str()); } } @@ -297,7 +294,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false)) { // Dave's debacle - RCLCPP_INFO(logger_, "Trajectory component '%s' is invalid", + RCLCPP_INFO(LOGGER, "Trajectory component '%s' is invalid", plan.plan_components[path_segment.first].description.c_str()); // call the same functions again, in verbose mode, to show what issues have been detected @@ -337,7 +334,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni if (!trajectory_execution_manager_) { - RCLCPP_ERROR(logger_, "No trajectory execution manager"); + RCLCPP_ERROR(LOGGER, "No trajectory execution manager"); result.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED; return result; } @@ -402,7 +399,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni if (!trajectory_execution_manager_->push(msg, plan.plan_components[component_idx].controller_name)) { trajectory_execution_manager_->clear(); - RCLCPP_ERROR(logger_, "Apparently trajectory initialization failed"); + RCLCPP_ERROR(LOGGER, "Apparently trajectory initialization failed"); execution_complete_ = true; result.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED; return result; @@ -441,7 +438,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni std::pair current_index = trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex(); if (!isRemainingPathValid(plan, current_index)) { - RCLCPP_INFO(logger_, "Trajectory component '%s' is invalid after scene update", + RCLCPP_INFO(LOGGER, "Trajectory component '%s' is invalid after scene update", plan.plan_components[current_index.first].description.c_str()); path_became_invalid_ = true; break; @@ -456,19 +453,19 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni // stop execution if needed if (preempt_requested) { - RCLCPP_INFO(logger_, "Stopping execution due to preempt request"); + RCLCPP_INFO(LOGGER, "Stopping execution due to preempt request"); trajectory_execution_manager_->stopExecution(); } else if (path_became_invalid_) { - RCLCPP_INFO(logger_, "Stopping execution because the path to execute became invalid" - "(probably the environment changed)"); + RCLCPP_INFO(LOGGER, "Stopping execution because the path to execute became invalid" + "(probably the environment changed)"); trajectory_execution_manager_->stopExecution(); } else if (!execution_complete_) { - RCLCPP_WARN(logger_, "Stopping execution due to unknown reason." - "Possibly the node is about to shut down."); + RCLCPP_WARN(LOGGER, "Stopping execution due to unknown reason." + "Possibly the node is about to shut down."); trajectory_execution_manager_->stopExecution(); } @@ -532,18 +529,18 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E { if (plan.plan_components.empty()) { - RCLCPP_WARN(logger_, "Length of provided motion plan is zero."); + RCLCPP_WARN(LOGGER, "Length of provided motion plan is zero."); return; } // if any side-effects are associated to the trajectory part that just completed, execute them - RCLCPP_DEBUG(logger_, "Completed '%s'", plan.plan_components[index].description.c_str()); + RCLCPP_DEBUG(LOGGER, "Completed '%s'", plan.plan_components[index].description.c_str()); if (plan.plan_components[index].effect_on_success) { if (!plan.plan_components[index].effect_on_success(&plan)) { // execution of side-effect failed - RCLCPP_ERROR(logger_, "Execution of path-completion side-effect failed. Preempting."); + RCLCPP_ERROR(LOGGER, "Execution of path-completion side-effect failed. Preempting."); preempt_.request(); return; } @@ -557,7 +554,7 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E std::pair next_index(static_cast(index), 0); if (!isRemainingPathValid(plan, next_index)) { - RCLCPP_INFO(logger_, "Upcoming trajectory component '%s' is invalid", + RCLCPP_INFO(LOGGER, "Upcoming trajectory component '%s' is invalid", plan.plan_components[next_index.first].description.c_str()); path_became_invalid_ = true; } 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..b77d9ab388 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 @@ -36,7 +36,6 @@ #include #include -#include using namespace std::chrono_literals; @@ -46,7 +45,6 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("display_random_state"); - moveit::setLogger(node->get_logger()); bool valid = false; bool invalid = false; @@ -81,7 +79,7 @@ int main(int argc, char** argv) { if (!psm.getPlanningScene()) { - RCLCPP_ERROR(moveit::getLogger(), "Planning scene did not load properly, exiting..."); + RCLCPP_ERROR(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..add1a93c5d 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 @@ -39,17 +39,17 @@ #include #include #include -#include -using moveit::getLogger; using namespace std::chrono_literals; static const std::string ROBOT_DESCRIPTION = "robot_description"; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("evaluate_collision_checking_speed"); + void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene& scene, const moveit::core::RobotState& state) { - RCLCPP_INFO(getLogger(), "Starting thread %u", id); + RCLCPP_INFO(LOGGER, "Starting thread %u", id); rclcpp::Clock clock(RCL_ROS_TIME); collision_detection::CollisionRequest req; rclcpp::Time start = clock.now(); @@ -59,15 +59,13 @@ void runCollisionDetection(unsigned int id, unsigned int trials, const planning_ scene.checkCollision(req, res, state); } double duration = (clock.now() - start).seconds(); - RCLCPP_INFO(getLogger(), "Thread %u performed %lf collision checks per second", id, - static_cast(trials) / duration); + RCLCPP_INFO(LOGGER, "Thread %u performed %lf collision checks per second", id, static_cast(trials) / duration); } 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()); unsigned int nthreads = 2; unsigned int trials = 10000; @@ -106,7 +104,7 @@ int main(int argc, char** argv) rclcpp::sleep_for(500ms); std::vector states; - RCLCPP_INFO(getLogger(), "Sampling %u valid states...", nthreads); + RCLCPP_INFO(LOGGER, "Sampling %u valid states...", nthreads); for (unsigned int i = 0; i < nthreads; ++i) { // sample a valid state @@ -140,7 +138,7 @@ int main(int argc, char** argv) } } else - RCLCPP_ERROR(getLogger(), "Planning scene not configured"); + RCLCPP_ERROR(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..dd7726a66a 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 @@ -38,7 +38,6 @@ #include #include #include -#include static const std::string ROBOT_DESCRIPTION = "robot_description"; @@ -48,7 +47,6 @@ 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()); 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..463c397308 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 @@ -43,27 +43,26 @@ #include #include #include -#include static const std::string ROBOT_DESCRIPTION = "robot_description"; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("print_planning_scene_info"); 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()); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); - RCLCPP_INFO_STREAM(moveit::getLogger(), "Getting planning scene info to print"); + RCLCPP_INFO_STREAM(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(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..cfe6719ded 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 @@ -42,7 +42,6 @@ #include #include #include -#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #else @@ -51,12 +50,12 @@ #include using namespace std::chrono_literals; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("publish_scene_from_text"); int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("publish_planning_scene"); - moveit::setLogger(node->get_logger()); // decide whether to publish the full scene bool full_scene = false; @@ -101,7 +100,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(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 +123,7 @@ int main(int argc, char** argv) } else { - RCLCPP_WARN(moveit::getLogger(), + RCLCPP_WARN(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..5d365cd99d 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 @@ -40,7 +40,6 @@ #include #include #include -#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #else @@ -56,7 +55,6 @@ 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()); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index faf93daa44..96c0c0ab21 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -186,8 +186,6 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline /// Publish contacts if the generated plans are checked again by the planning pipeline rclcpp::Publisher::SharedPtr contacts_publisher_; - - rclcpp::Logger logger_; }; MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 10548bf919..24f31c7cb6 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -41,18 +41,18 @@ #include #include #include -#include #include +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); +} // namespace + planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace) - : active_{ false } - , node_(node) - , parameter_namespace_(parameter_namespace) - , robot_model_(model) - , logger_(moveit::makeChildLogger("planning_pipeline")) + : active_{ false }, node_(node), parameter_namespace_(parameter_namespace), robot_model_(model) { auto param_listener = planning_pipeline_parameters::ParamListener(node, parameter_namespace); const auto params = param_listener.get_params(); @@ -81,7 +81,6 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM , planner_plugin_name_(planner_plugin_name) , adapter_plugin_names_(adapter_plugin_names) , robot_model_(model) - , logger_(moveit::makeChildLogger("planning_pipeline")) { configure(); } @@ -104,7 +103,7 @@ void planning_pipeline::PlanningPipeline::configure() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL(logger_, "Exception while creating planning plugin loader %s", ex.what()); + RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what()); throw; } @@ -122,12 +121,12 @@ void planning_pipeline::PlanningPipeline::configure() { throw std::runtime_error("Unable to initialize planning plugin"); } - RCLCPP_INFO(logger_, "Using planning interface '%s'", planner_instance_->getDescription().c_str()); + RCLCPP_INFO(LOGGER, "Using planning interface '%s'", planner_instance_->getDescription().c_str()); } catch (pluginlib::PluginlibException& ex) { std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); - RCLCPP_FATAL(logger_, + RCLCPP_FATAL(LOGGER, "Exception while loading planner '%s': %s" "Available plugins: %s", planner_plugin_name_.c_str(), ex.what(), classes_str.c_str()); @@ -146,7 +145,7 @@ void planning_pipeline::PlanningPipeline::configure() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL(logger_, "Exception while creating planning plugin loader %s", ex.what()); + RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what()); throw; } @@ -161,7 +160,7 @@ void planning_pipeline::PlanningPipeline::configure() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL(logger_, "Exception while loading planning adapter plugin '%s': %s", adapter_plugin_name.c_str(), + RCLCPP_FATAL(LOGGER, "Exception while loading planning adapter plugin '%s': %s", adapter_plugin_name.c_str(), ex.what()); throw; } @@ -178,14 +177,14 @@ void planning_pipeline::PlanningPipeline::configure() for (planning_request_adapter::PlanningRequestAdapterConstPtr& planning_request_adapter : planning_request_adapter_vector) { - RCLCPP_INFO(logger_, "Using planning request adapter '%s'", planning_request_adapter->getDescription().c_str()); + RCLCPP_INFO(LOGGER, "Using planning request adapter '%s'", planning_request_adapter->getDescription().c_str()); adapter_chain_->addAdapter(planning_request_adapter); } } } else { - RCLCPP_WARN(logger_, "No planning request adapter names specified."); + RCLCPP_WARN(LOGGER, "No planning request adapter names specified."); } } @@ -221,7 +220,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla std::stringstream ss; for (std::size_t added_index : res.added_path_index) ss << added_index << ' '; - RCLCPP_INFO(logger_, "Planning adapters have added states at index positions: [ %s]", ss.str().c_str()); + RCLCPP_INFO(LOGGER, "Planning adapters have added states at index positions: [ %s]", ss.str().c_str()); } } else @@ -233,7 +232,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Exception caught: '%s'", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught: '%s'", ex.what()); // Set planning pipeline to inactive active_ = false; return false; @@ -245,7 +244,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla if (solved && res.trajectory) { std::size_t state_count = res.trajectory->getWayPointCount(); - RCLCPP_DEBUG(logger_, "Motion planner reported a solution path with %ld states", state_count); + RCLCPP_DEBUG(LOGGER, "Motion planner reported a solution path with %ld states", state_count); if (check_solution_paths) { visualization_msgs::msg::MarkerArray arr; @@ -280,7 +279,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla { if (indices.size() == 1 && indices.at(0) == 0) { // ignore cases when the robot starts at invalid location - RCLCPP_DEBUG(logger_, "It appears the robot is starting at an invalid state, but that is ok."); + RCLCPP_DEBUG(LOGGER, "It appears the robot is starting at an invalid state, but that is ok."); } else { @@ -293,10 +292,10 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla ss << it << ' '; } - RCLCPP_ERROR_STREAM(logger_, "Computed path is not valid. Invalid states at index locations: [ " - << ss.str() << "] out of " << state_count - << ". Explanations follow in command line. Contacts are published on " - << contacts_publisher_->get_topic_name()); + RCLCPP_ERROR_STREAM(LOGGER, "Computed path is not valid. Invalid states at index locations: [ " + << ss.str() << "] out of " << state_count + << ". Explanations follow in command line. Contacts are published on " + << contacts_publisher_->get_topic_name()); // call validity checks in verbose mode for the problematic states for (std::size_t it : indices) @@ -321,18 +320,18 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end()); } } - RCLCPP_ERROR(logger_, "Completed listing of explanations for invalid states."); + RCLCPP_ERROR(LOGGER, "Completed listing of explanations for invalid states."); } } else { - RCLCPP_DEBUG(logger_, + RCLCPP_DEBUG(LOGGER, "Planned path was found to be valid, except for states that were added by planning request " "adapters, but that is ok."); } } else - RCLCPP_DEBUG(logger_, "Planned path was found to be valid when rechecked"); + RCLCPP_DEBUG(LOGGER, "Planned path was found to be valid when rechecked"); contacts_publisher_->publish(arr); } @@ -365,20 +364,19 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } if (stacked_constraints) { - RCLCPP_WARN(logger_, "More than one constraint is set. If your move_group does not have multiple end " - "effectors/arms, this is " - "unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or " - "equivalent?"); + RCLCPP_WARN(LOGGER, "More than one constraint is set. If your move_group does not have multiple end " + "effectors/arms, this is " + "unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or " + "equivalent?"); } } // Make sure that planner id is set in the response if (res.planner_id.empty()) { - RCLCPP_WARN(logger_, - "The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting " - "it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn " - "you if it does not use the requested planner."); + RCLCPP_WARN(LOGGER, "The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting " + "it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn " + "you if it does not use the requested planner."); res.planner_id = req.planner_id; } 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..af37378281 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 @@ -35,7 +35,6 @@ /* Author: Sebastian Jahr */ #include -#include #include @@ -43,12 +42,7 @@ namespace moveit { namespace planning_pipeline_interfaces { - -rclcpp::Logger get_logger() -{ - static rclcpp::Logger logger = moveit::makeChildLogger("planning_pipeline_interfaces"); - return logger; -} +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.::planning_interface.planning_pipeline_interfaces"); ::planning_interface::MotionPlanResponse planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_plan_request, @@ -59,7 +53,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(LOGGER, "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 +82,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(LOGGER, "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " "hardware ('%d')", motion_plan_requests.size(), hardware_concurrency); @@ -106,7 +100,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(LOGGER, "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 +112,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(LOGGER, "Stopping criterion met: Terminating planning pipelines that are still active"); for (const auto& request : motion_plan_requests) { try @@ -131,7 +125,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(LOGGER, "Cannot terminate pipeline '%s' because no pipeline with that name exists", request.pipeline_id.c_str()); } } @@ -174,7 +168,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(LOGGER, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } @@ -184,7 +178,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(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index 9e17b0daf0..2e3638397b 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -45,6 +45,8 @@ namespace default_planner_request_adapters { using namespace trajectory_processing; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_traj_smoothing"); + /** @brief Use ruckig (https://github.com/pantor/ruckig) to adapt the trajectory to be jerk-constrained and * time-optimal.*/ class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRequestAdapter diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index ad0a7701c8..677494212c 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include @@ -45,14 +44,12 @@ namespace default_planner_request_adapters { using namespace trajectory_processing; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_time_optimal_parameterization"); + /** @brief This adapter uses the time-optimal trajectory generation method */ class AddTimeOptimalParameterization : public planning_request_adapter::PlanningRequestAdapter { public: - AddTimeOptimalParameterization() : logger_(moveit::makeChildLogger("add_time_optimal_parameterization")) - { - } - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -71,12 +68,12 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning bool result = planner(planning_scene, req, res); if (result && res.trajectory) { - RCLCPP_DEBUG(logger_, " Running '%s'", getDescription().c_str()); + RCLCPP_DEBUG(LOGGER, " Running '%s'", getDescription().c_str()); const auto params = param_listener_->get_params(); TimeOptimalTrajectoryGeneration totg(params.path_tolerance, params.resample_dt, params.min_angle_change); if (!totg.computeTimeStamps(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { - RCLCPP_WARN(logger_, " Time parametrization for the solution path failed."); + RCLCPP_WARN(LOGGER, " Time parametrization for the solution path failed."); result = false; } } @@ -86,7 +83,6 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning protected: std::unique_ptr param_listener_; - rclcpp::Logger logger_; }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index 40f82076fa..ae755e8320 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -52,21 +52,17 @@ #include #include #include -#include #include namespace default_planner_request_adapters { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_state_bounds"); /** @brief Fix start state bounds adapter fixes the start state to be within the joint limits specified in the URDF. */ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdapter { public: - FixStartStateBounds() : logger_(moveit::makeChildLogger("fix_start_state_bounds")) - { - } - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -82,7 +78,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const override { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); // get the specified start state moveit::core::RobotState start_state = planning_scene->getCurrentState(); @@ -154,7 +150,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap prefix_state = std::make_shared(start_state); start_state.enforceBounds(jmodel); change_req = true; - RCLCPP_INFO(logger_, "Starting state is just outside bounds (joint '%s'). Assuming within bounds.", + RCLCPP_INFO(LOGGER, "Starting state is just outside bounds (joint '%s'). Assuming within bounds.", jmodel->getName().c_str()); } else @@ -171,7 +167,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap joint_bounds_low << variable_bounds.min_position_ << ' '; joint_bounds_hi << variable_bounds.max_position_ << ' '; } - RCLCPP_WARN(logger_, + RCLCPP_WARN(LOGGER, "Joint '%s' from the starting state is outside bounds by a significant margin: [%s] should be in " "the range [%s], [%s] but the error above the 'start_state_max_bounds_error' parameter " "(currently set to %f)", @@ -213,7 +209,6 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap private: std::unique_ptr param_listener_; - rclcpp::Logger logger_; }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index e554feb2a1..b58196d059 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -48,22 +48,18 @@ #include #include #include -#include #include namespace default_planner_request_adapters { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_state_collision"); /** @brief This fix start state collision adapter will attempt to sample a new collision-free configuration near a * specified configuration (in collision) by perturbing the joint values by a small amount.*/ class FixStartStateCollision : public planning_request_adapter::PlanningRequestAdapter { public: - FixStartStateCollision() : logger_(moveit::makeChildLogger("fix_start_state_collision")) - { - } - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -79,7 +75,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const override { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); // get the specified start state moveit::core::RobotState start_state = planning_scene->getCurrentState(); @@ -99,11 +95,11 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA if (creq.group_name.empty()) { - RCLCPP_INFO(logger_, "Start state appears to be in collision"); + RCLCPP_INFO(LOGGER, "Start state appears to be in collision"); } else { - RCLCPP_INFO(logger_, "Start state appears to be in collision with respect to group %s", creq.group_name.c_str()); + RCLCPP_INFO(LOGGER, "Start state appears to be in collision with respect to group %s", creq.group_name.c_str()); } auto prefix_state = std::make_shared(start_state); @@ -131,7 +127,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA if (!cres.collision) { found = true; - RCLCPP_INFO(logger_, "Found a valid state near the start state at distance %lf after %d attempts", + RCLCPP_INFO(LOGGER, "Found a valid state near the start state at distance %lf after %d attempts", prefix_state->distance(start_state), c); } } @@ -161,7 +157,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA else { RCLCPP_WARN( - logger_, + LOGGER, "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %lu sampling " "attempts). Passing the original planning request to the planner.", params.jiggle_fraction, params.max_sampling_attempts); @@ -173,11 +169,11 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA { if (creq.group_name.empty()) { - RCLCPP_DEBUG(logger_, "Start state is valid"); + RCLCPP_DEBUG(LOGGER, "Start state is valid"); } else { - RCLCPP_DEBUG(logger_, "Start state is valid with respect to group %s", creq.group_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Start state is valid with respect to group %s", creq.group_name.c_str()); } return planner(planning_scene, req, res); } @@ -185,7 +181,6 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA private: std::unique_ptr param_listener_; - rclcpp::Logger logger_; }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index 956eedb496..3453dd0ec1 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -48,10 +48,10 @@ #include #include #include -#include namespace default_planner_request_adapters { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_state_path_constraints"); /** @brief This fix start state collision adapter will attempt to sample a new collision-free configuration near a * specified configuration (in collision) by perturbing the joint values by a small amount.*/ @@ -59,10 +59,6 @@ namespace default_planner_request_adapters class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter { public: - FixStartStatePathConstraints() : logger_(moveit::makeChildLogger("fix_start_state_path_constraints")) - { - } - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } @@ -76,7 +72,7 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const override { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); // get the specified start state moveit::core::RobotState start_state = planning_scene->getCurrentState(); @@ -86,9 +82,9 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe if (planning_scene->isStateValid(start_state, req.group_name) && !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name)) { - RCLCPP_INFO(logger_, "Path constraints not satisfied for start state..."); + RCLCPP_INFO(LOGGER, "Path constraints not satisfied for start state..."); planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true); - RCLCPP_INFO(logger_, "Planning to path constraints..."); + RCLCPP_INFO(LOGGER, "Planning to path constraints..."); planning_interface::MotionPlanRequest req2 = req; req2.goal_constraints.resize(1); @@ -105,7 +101,7 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe if (solved1) { planning_interface::MotionPlanRequest req3 = req; - RCLCPP_INFO(logger_, "Planned to path constraints. Resuming original planning request."); + RCLCPP_INFO(LOGGER, "Planned to path constraints. Resuming original planning request."); // extract the last state of the computed motion plan and set it as the new start state moveit::core::robotStateToRobotStateMsg(res2.trajectory->getLastWayPoint(), req3.start_state); @@ -138,7 +134,7 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe } else { - RCLCPP_WARN(logger_, "Unable to plan to path constraints. Running usual motion plan."); + RCLCPP_WARN(LOGGER, "Unable to plan to path constraints. Running usual motion plan."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS; res.planning_time = res2.planning_time; return false; // skip remaining adapters and/or planner @@ -146,13 +142,10 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe } else { - RCLCPP_DEBUG(logger_, "Path constraints are OK. Running usual motion plan."); + RCLCPP_DEBUG(LOGGER, "Path constraints are OK. Running usual motion plan."); return planner(planning_scene, req, res); } } - -private: - rclcpp::Logger logger_; }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp index 5fafe8d8c9..72c1bd924d 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp @@ -42,22 +42,18 @@ #include #include #include -#include #include namespace default_planner_request_adapters { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_workspace_bounds"); /** @brief This fix workspace bounds adapter will specify a default workspace for planning: a cube of size 10 m x 10 m x * 10 m. This workspace will only be specified if the planning request to the planner does not have these fields filled in. */ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapter { public: - FixWorkspaceBounds() : logger_(moveit::makeChildLogger("fix_workspace_bounds")) - { - } - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -73,13 +69,13 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const override { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); const moveit_msgs::msg::WorkspaceParameters& wparams = req.workspace_parameters; if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 && wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 && wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0) { - RCLCPP_DEBUG(logger_, "It looks like the planning volume was not specified. Using default values."); + RCLCPP_DEBUG(LOGGER, "It looks like the planning volume was not specified. Using default values."); planning_interface::MotionPlanRequest req2 = req; moveit_msgs::msg::WorkspaceParameters& default_wp = req2.workspace_parameters; const auto params = param_listener_->get_params(); @@ -98,7 +94,6 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt private: std::unique_ptr param_listener_; - rclcpp::Logger logger_; }; } // namespace default_planner_request_adapters 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 cc4ef32e08..212c69db8a 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 @@ -37,7 +37,6 @@ #include #include #include -#include namespace default_planner_request_adapters { @@ -46,10 +45,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.resolve_cons class ResolveConstraintFrames : public planning_request_adapter::PlanningRequestAdapter { public: - ResolveConstraintFrames() : logger_(moveit::makeChildLogger("resolve_constraint_frames")) - { - } - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } @@ -63,7 +58,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const override { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); planning_interface::MotionPlanRequest modified = req; kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), modified.path_constraints); for (moveit_msgs::msg::Constraints& constraint : modified.goal_constraints) @@ -72,9 +67,6 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest } return planner(planning_scene, modified, res); } - -private: - rclcpp::Logger logger_; }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp index 0e0c873de6..4b0a16e51e 100644 --- a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp +++ b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp @@ -51,6 +51,8 @@ #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_scene_monitor.demo_scene"); + static const std::string ROBOT_DESCRIPTION = "robot_description"; void sendKnife(const rclcpp::Node::SharedPtr& node) @@ -91,7 +93,7 @@ void sendKnife(const rclcpp::Node::SharedPtr& node) pub_aco->publish(aco); rclcpp::sleep_for(1s); pub_aco->publish(aco); - RCLCPP_INFO(node->get_logger(), "Object published."); + RCLCPP_INFO(LOGGER, "Object published."); rclcpp::sleep_for(1500ms); } diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index b1ff502d9c..1b4c9b4995 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -342,8 +342,6 @@ class CurrentStateMonitor std::vector update_callbacks_; bool use_sim_time_; - - rclcpp::Logger logger_; }; MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index fd5199a2e5..d41e6f2a50 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -592,8 +592,6 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor friend class LockedPlanningSceneRO; friend class LockedPlanningSceneRW; - - rclcpp::Logger logger_; }; /** \brief This is a convenience class for obtaining access to an diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index 9975093c06..d5e55feadc 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -140,7 +140,5 @@ class TrajectoryMonitor std::unique_ptr record_states_thread_; TrajectoryStateAddedCallback state_add_callback_; - - rclcpp::Logger logger_; }; } // namespace planning_scene_monitor 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..538c2a3c63 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 @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -49,6 +48,11 @@ namespace planning_scene_monitor { using namespace std::chrono_literals; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.current_state_monitor"); +} + CurrentStateMonitor::CurrentStateMonitor(std::unique_ptr middleware_handle, const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, bool use_sim_time) @@ -60,7 +64,6 @@ CurrentStateMonitor::CurrentStateMonitor(std::unique_ptr::epsilon()) , use_sim_time_(use_sim_time) - , logger_(moveit::makeChildLogger("current_state_monitor")) { robot_state_.setToDefaultValues(); } @@ -152,7 +155,7 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi joint_time_.clear(); if (joint_states_topic.empty()) { - RCLCPP_ERROR(logger_, "The joint states topic cannot be an empty string"); + RCLCPP_ERROR(LOGGER, "The joint states topic cannot be an empty string"); } else { @@ -169,7 +172,7 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi } state_monitor_started_ = true; monitor_start_time_ = middleware_handle_->now(); - RCLCPP_INFO(logger_, "Listening to joint states on topic '%s'", joint_states_topic.c_str()); + RCLCPP_INFO(LOGGER, "Listening to joint states on topic '%s'", joint_states_topic.c_str()); } } @@ -187,7 +190,7 @@ void CurrentStateMonitor::stopStateMonitor() { middleware_handle_->resetTfSubscriptions(); } - RCLCPP_DEBUG(logger_, "No longer listening for joint states"); + RCLCPP_DEBUG(LOGGER, "No longer listening for joint states"); state_monitor_started_ = false; } } @@ -207,12 +210,12 @@ bool CurrentStateMonitor::haveCompleteStateHelper(const rclcpp::Time& oldest_all std::map::const_iterator it = joint_time_.find(joint); if (it == joint_time_.end()) { - RCLCPP_DEBUG(logger_, "Joint '%s' has never been updated", joint->getName().c_str()); + RCLCPP_DEBUG(LOGGER, "Joint '%s' has never been updated", joint->getName().c_str()); } else if (it->second < oldest_allowed_update_time) { - RCLCPP_DEBUG(logger_, "Joint '%s' was last updated %0.3lf seconds before requested time", - joint->getName().c_str(), (oldest_allowed_update_time - it->second).seconds()); + RCLCPP_DEBUG(LOGGER, "Joint '%s' was last updated %0.3lf seconds before requested time", joint->getName().c_str(), + (oldest_allowed_update_time - it->second).seconds()); } else continue; @@ -247,7 +250,7 @@ bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait * state messages, warn the user. */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, steady_clock, 1000, + RCLCPP_WARN_SKIPFIRST_THROTTLE(LOGGER, steady_clock, 1000, "No state update received within 100ms of system clock. " "Have been waiting for %fs, timeout is %fs", elapsed.seconds(), wait_time_s); @@ -261,7 +264,7 @@ bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait elapsed = middleware_handle_->now() - start; if (elapsed > timeout) { - RCLCPP_INFO(logger_, + RCLCPP_INFO(LOGGER, "Didn't receive robot state (joint angles) with recent timestamp within " "%f seconds. Requested time %f, but latest received state has time %f.\n" "Check clock synchronization if your are running ROS across multiple machines!", @@ -270,7 +273,7 @@ bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait } if (!middleware_handle_->ok()) { - RCLCPP_DEBUG(logger_, "ROS context shut down while waiting for current robot state."); + RCLCPP_DEBUG(LOGGER, "ROS context shut down while waiting for current robot state."); return false; } } @@ -326,7 +329,7 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::msg::JointState: rclcpp::Clock steady_clock(RCL_STEADY_TIME); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(logger_, steady_clock, 1000, + RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000, "State monitor received invalid joint state (number of joint names does not match number of " "positions)"); #pragma GCC diagnostic pop @@ -438,7 +441,7 @@ void CurrentStateMonitor::updateMultiDofJoints() } catch (tf2::TransformException& ex) { - RCLCPP_WARN_ONCE(logger_, + RCLCPP_WARN_ONCE(LOGGER, "Unable to update multi-DOF joint '%s':" "Failure to lookup transform between '%s'" "and '%s' with TF exception: %s", @@ -508,7 +511,7 @@ void CurrentStateMonitor::transformCallback(const tf2_msgs::msg::TFMessage::Cons catch (tf2::TransformException& ex) { std::string temp = ex.what(); - RCLCPP_ERROR(logger_, "Failure to set received transform from %s to %s with error: %s\n", + RCLCPP_ERROR(LOGGER, "Failure to set received transform from %s to %s with error: %s\n", transform.child_frame_id.c_str(), transform.header.frame_id.c_str(), temp.c_str()); } } 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 72ae9cf704..4e53f8756f 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 @@ -39,7 +39,6 @@ #include #include #include -#include #include #include @@ -54,6 +53,8 @@ #include using namespace std::chrono_literals; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_scene_monitor.planning_scene_monitor"); + namespace planning_scene_monitor { const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states"; @@ -96,7 +97,6 @@ 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")) { std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -173,7 +173,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc } catch (moveit::ConstructException& e) { - RCLCPP_ERROR(logger_, "Configuration of planning scene failed"); + RCLCPP_ERROR(LOGGER, "Configuration of planning scene failed"); scene_.reset(); } } @@ -194,7 +194,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc } else { - RCLCPP_ERROR(logger_, "Robot model not loaded"); + RCLCPP_ERROR(LOGGER, "Robot model not loaded"); } publish_planning_scene_frequency_ = 2.0; @@ -276,8 +276,8 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc } catch (const rclcpp::exceptions::InvalidParameterTypeException& e) { - RCLCPP_ERROR_STREAM(logger_, "Invalid parameter type in PlanningSceneMonitor: " << e.what()); - RCLCPP_ERROR(logger_, "Dynamic publishing parameters won't be available"); + RCLCPP_ERROR_STREAM(LOGGER, "Invalid parameter type in PlanningSceneMonitor: " << e.what()); + RCLCPP_ERROR(LOGGER, "Dynamic publishing parameters won't be available"); return; } @@ -297,7 +297,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc if (!declared_params_valid) { - RCLCPP_ERROR(logger_, "Initially declared parameters are invalid - failed to process update callback"); + RCLCPP_ERROR(LOGGER, "Initially declared parameters are invalid - failed to process update callback"); result.successful = false; return result; } @@ -310,7 +310,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc // Only allow already declared parameters with same value type if (!pnode_->has_parameter(name) || pnode_->get_parameter(name).get_type() != type) { - RCLCPP_ERROR(logger_, "Invalid parameter in PlanningSceneMonitor parameter callback"); + RCLCPP_ERROR(LOGGER, "Invalid parameter in PlanningSceneMonitor parameter callback"); result.successful = false; return result; } @@ -376,8 +376,8 @@ void PlanningSceneMonitor::monitorDiffs(bool flag) { if (publish_planning_scene_) { - RCLCPP_WARN(logger_, "Diff monitoring was stopped while publishing planning scene diffs. " - "Stopping planning scene diff publisher"); + RCLCPP_WARN(LOGGER, "Diff monitoring was stopped while publishing planning scene diffs. " + "Stopping planning scene diff publisher"); stopPublishingPlanningScene(); } { @@ -408,7 +408,7 @@ void PlanningSceneMonitor::stopPublishingPlanningScene() copy->join(); monitorDiffs(false); planning_scene_publisher_.reset(); - RCLCPP_INFO(logger_, "Stopped publishing maintained planning scene."); + RCLCPP_INFO(LOGGER, "Stopped publishing maintained planning scene."); } } @@ -419,7 +419,7 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t if (!publish_planning_scene_ && scene_) { planning_scene_publisher_ = pnode_->create_publisher(planning_scene_topic, 100); - RCLCPP_INFO(logger_, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str()); + RCLCPP_INFO(LOGGER, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str()); monitorDiffs(true); publish_planning_scene_ = std::make_unique([this] { scenePublishingThread(); }); } @@ -427,7 +427,7 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t void PlanningSceneMonitor::scenePublishingThread() { - RCLCPP_DEBUG(logger_, "Started scene publishing thread ..."); + RCLCPP_DEBUG(LOGGER, "Started scene publishing thread ..."); // publish the full planning scene once { @@ -439,7 +439,7 @@ void PlanningSceneMonitor::scenePublishingThread() scene_->getPlanningSceneMsg(msg); } planning_scene_publisher_->publish(msg); - RCLCPP_DEBUG(logger_, "Published the full planning scene: '%s'", msg.name.c_str()); + RCLCPP_DEBUG(LOGGER, "Published the full planning scene: '%s'", msg.name.c_str()); } do @@ -510,7 +510,7 @@ void PlanningSceneMonitor::scenePublishingThread() { planning_scene_publisher_->publish(msg); if (is_full) - RCLCPP_DEBUG(logger_, "Published full planning scene: '%s'", msg.name.c_str()); + RCLCPP_DEBUG(LOGGER, "Published full planning scene: '%s'", msg.name.c_str()); rate.sleep(); } } while (publish_planning_scene_); @@ -576,7 +576,7 @@ bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_ { if (get_scene_service_ && get_scene_service_->get_service_name() == service_name) { - RCLCPP_FATAL_STREAM(logger_, "requestPlanningSceneState() to self-provided service '" << service_name << '\''); + RCLCPP_FATAL_STREAM(LOGGER, "requestPlanningSceneState() to self-provided service '" << service_name << '\''); throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name); } // use global namespace for service @@ -589,28 +589,28 @@ bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_ srv->components.LINK_PADDING_AND_SCALING | srv->components.OBJECT_COLORS; // Make sure client is connected to server - RCLCPP_DEBUG(logger_, "Waiting for GetPlanningScene service `%s` to exist.", service_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Waiting for GetPlanningScene service `%s` to exist.", service_name.c_str()); if (client->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_DEBUG(logger_, "Sending service request to `%s`.", service_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Sending service request to `%s`.", service_name.c_str()); auto service_result = client->async_send_request(srv); const auto service_result_status = service_result.wait_for(std::chrono::seconds(5)); if (service_result_status == std::future_status::ready) // Success { - RCLCPP_DEBUG(logger_, "Service request succeeded, applying new planning scene"); + RCLCPP_DEBUG(LOGGER, "Service request succeeded, applying new planning scene"); newPlanningSceneMessage(service_result.get()->scene); return true; } if (service_result_status == std::future_status::timeout) // Timeout { - RCLCPP_INFO(logger_, "GetPlanningScene service call to %s timed out. at %s:%d", service_name.c_str(), __FILE__, + RCLCPP_INFO(LOGGER, "GetPlanningScene service call to %s timed out. at %s:%d", service_name.c_str(), __FILE__, __LINE__); return false; } } // If we are here, service is not available or call failed - RCLCPP_INFO(logger_, + RCLCPP_INFO(LOGGER, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?", service_name.c_str()); @@ -690,7 +690,7 @@ void PlanningSceneMonitor::clearOctomap() } else { - RCLCPP_WARN(logger_, "Unable to clear octomap since no octomap monitor has been initialized"); + RCLCPP_WARN(LOGGER, "Unable to clear octomap since no octomap monitor has been initialized"); } // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock } @@ -714,7 +714,7 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann last_update_time_ = rclcpp::Clock().now(); last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp; - RCLCPP_DEBUG(logger_, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.), + RCLCPP_DEBUG(LOGGER, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.), fmod(last_robot_motion_time_.seconds(), 10.)); old_scene_name = scene_->getName(); result = scene_->usePlanningSceneMsg(scene); @@ -865,7 +865,7 @@ void PlanningSceneMonitor::excludeRobotLinksFromOctree() if (!warned && ((std::chrono::system_clock::now() - start) > std::chrono::seconds(30))) { - RCLCPP_WARN(logger_, "It is likely there are too many vertices in collision geometry"); + RCLCPP_WARN(LOGGER, "It is likely there are too many vertices in collision geometry"); warned = true; } } @@ -964,7 +964,7 @@ void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const moveit::core::Att } } if (found) - RCLCPP_DEBUG(logger_, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str()); + RCLCPP_DEBUG(LOGGER, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str()); } void PlanningSceneMonitor::includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body) @@ -979,7 +979,7 @@ void PlanningSceneMonitor::includeAttachedBodyInOctree(const moveit::core::Attac { for (std::pair& shape_handle : it->second) octomap_monitor_->forgetShape(shape_handle.first); - RCLCPP_DEBUG(logger_, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str()); + RCLCPP_DEBUG(LOGGER, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str()); attached_body_shape_handles_.erase(it); } } @@ -1004,7 +1004,7 @@ void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detectio } } if (found) - RCLCPP_DEBUG(logger_, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str()); + RCLCPP_DEBUG(LOGGER, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str()); } void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj) @@ -1019,7 +1019,7 @@ void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection: { for (std::pair& shape_handle : it->second) octomap_monitor_->forgetShape(shape_handle.first); - RCLCPP_DEBUG(logger_, "Including collision object '%s' in monitored octomap", obj->id_.c_str()); + RCLCPP_DEBUG(LOGGER, "Including collision object '%s' in monitored octomap", obj->id_.c_str()); collision_body_shape_handles_.erase(it); } } @@ -1067,7 +1067,7 @@ bool PlanningSceneMonitor::waitForCurrentRobotState(const rclcpp::Time& t, doubl { if (t.nanoseconds() == 0) return false; - RCLCPP_DEBUG(logger_, "sync robot state to: %.3fs", fmod(t.seconds(), 10.)); + RCLCPP_DEBUG(LOGGER, "sync robot state to: %.3fs", fmod(t.seconds(), 10.)); if (current_state_monitor_) { @@ -1090,7 +1090,7 @@ bool PlanningSceneMonitor::waitForCurrentRobotState(const rclcpp::Time& t, doubl return true; } - RCLCPP_WARN(logger_, "Failed to fetch current robot state."); + RCLCPP_WARN(LOGGER, "Failed to fetch current robot state."); return false; } @@ -1104,19 +1104,16 @@ bool PlanningSceneMonitor::waitForCurrentRobotState(const rclcpp::Time& t, doubl while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene. timeout > rclcpp::Duration(0, 0)) { - RCLCPP_DEBUG(logger_, "last robot motion: %f ago", (t - last_robot_motion_time_).seconds()); + RCLCPP_DEBUG(LOGGER, "last robot motion: %f ago", (t - last_robot_motion_time_).seconds()); new_scene_update_condition_.wait_for(lock, std::chrono::nanoseconds(timeout.nanoseconds())); timeout = timeout - (node_->get_clock()->now() - start); // compute remaining wait_time } bool success = last_robot_motion_time_ >= t; // suppress warning if we received an update at all if (!success && prev_robot_motion_time != last_robot_motion_time_) - { - RCLCPP_WARN(logger_, "Maybe failed to update robot state, time diff: %.3fs", - (t - last_robot_motion_time_).seconds()); - } + RCLCPP_WARN(LOGGER, "Maybe failed to update robot state, time diff: %.3fs", (t - last_robot_motion_time_).seconds()); - RCLCPP_DEBUG(logger_, "sync done: robot motion: %f scene update: %f", (t - last_robot_motion_time_).seconds(), + RCLCPP_DEBUG(LOGGER, "sync done: robot motion: %f scene update: %f", (t - last_robot_motion_time_).seconds(), (t - last_update_time_).seconds()); return success; } @@ -1153,7 +1150,7 @@ void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic) { stopSceneMonitor(); - RCLCPP_INFO(logger_, "Starting planning scene monitor"); + RCLCPP_INFO(LOGGER, "Starting planning scene monitor"); // listen for planning scene updates; these messages include transforms, so no need for filters if (!scene_topic.empty()) { @@ -1161,7 +1158,7 @@ void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic) scene_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) { return newPlanningSceneCallback(scene); }); - RCLCPP_INFO(logger_, "Listening to '%s'", planning_scene_subscriber_->get_topic_name()); + RCLCPP_INFO(LOGGER, "Listening to '%s'", planning_scene_subscriber_->get_topic_name()); } } @@ -1169,7 +1166,7 @@ void PlanningSceneMonitor::stopSceneMonitor() { if (planning_scene_subscriber_) { - RCLCPP_INFO(logger_, "Stopping planning scene monitor"); + RCLCPP_INFO(LOGGER, "Stopping planning scene monitor"); planning_scene_subscriber_.reset(); } } @@ -1236,7 +1233,7 @@ bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_fram rclcpp::Clock steady_clock = rclcpp::Clock(); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(logger_, steady_clock, 1000, "Transform error: %s", ex.what()); + RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000, "Transform error: %s", ex.what()); #pragma GCC diagnostic pop return false; } @@ -1248,8 +1245,8 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio const bool load_octomap_monitor) { stopWorldGeometryMonitor(); - RCLCPP_INFO(logger_, "Starting world geometry update monitor for collision objects, attached objects, octomap " - "updates."); + RCLCPP_INFO(LOGGER, "Starting world geometry update monitor for collision objects, attached objects, octomap " + "updates."); // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world if (!collision_objects_topic.empty()) @@ -1257,7 +1254,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio collision_object_subscriber_ = pnode_->create_subscription( collision_objects_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); }); - RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str()); + RCLCPP_INFO(LOGGER, "Listening to '%s'", collision_objects_topic.c_str()); } if (!planning_scene_world_topic.empty()) @@ -1267,7 +1264,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { return newPlanningSceneWorldCallback(world); }); - RCLCPP_INFO(logger_, "Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str()); + RCLCPP_INFO(LOGGER, "Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str()); } // Ocotomap monitor is optional @@ -1295,12 +1292,12 @@ void PlanningSceneMonitor::stopWorldGeometryMonitor() { if (collision_object_subscriber_) { - RCLCPP_INFO(logger_, "Stopping world geometry monitor"); + RCLCPP_INFO(LOGGER, "Stopping world geometry monitor"); collision_object_subscriber_.reset(); } else if (planning_scene_world_subscriber_) { - RCLCPP_INFO(logger_, "Stopping world geometry monitor"); + RCLCPP_INFO(LOGGER, "Stopping world geometry monitor"); planning_scene_world_subscriber_.reset(); } if (octomap_monitor_) @@ -1341,12 +1338,12 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { return attachObjectCallback(obj); }); - RCLCPP_INFO(logger_, "Listening to '%s' for attached collision objects", + RCLCPP_INFO(LOGGER, "Listening to '%s' for attached collision objects", attached_collision_object_subscriber_->get_topic_name()); } } else - RCLCPP_ERROR(logger_, "Cannot monitor robot state because planning scene is not configured"); + RCLCPP_ERROR(LOGGER, "Cannot monitor robot state because planning scene is not configured"); } void PlanningSceneMonitor::stopStateMonitor() @@ -1408,7 +1405,7 @@ void PlanningSceneMonitor::stateUpdateTimerCallback() last_robot_state_update_wall_time_ = std::chrono::system_clock::now(); auto sec = std::chrono::duration(last_robot_state_update_wall_time_.time_since_epoch()).count(); update = true; - RCLCPP_DEBUG(logger_, "performPendingStateUpdate: %f", fmod(sec, 10)); + RCLCPP_DEBUG(LOGGER, "performPendingStateUpdate: %f", fmod(sec, 10)); } } @@ -1416,7 +1413,7 @@ void PlanningSceneMonitor::stateUpdateTimerCallback() if (update) { updateSceneWithCurrentState(); - RCLCPP_DEBUG(logger_, "performPendingStateUpdate done"); + RCLCPP_DEBUG(LOGGER, "performPendingStateUpdate done"); } } } @@ -1468,7 +1465,7 @@ void PlanningSceneMonitor::setStateUpdateFrequency(double hz) if (state_update_pending_) update = true; } - RCLCPP_INFO(logger_, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.count()); + RCLCPP_INFO(LOGGER, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.count()); if (update) updateSceneWithCurrentState(); @@ -1487,7 +1484,7 @@ void PlanningSceneMonitor::updateSceneWithCurrentState() std::string missing_str = fmt::format("{}", fmt::join(missing, ", ")); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_THROTTLE(logger_, steady_clock, 1000, "The complete state of the robot is not yet known. Missing %s", + RCLCPP_WARN_THROTTLE(LOGGER, steady_clock, 1000, "The complete state of the robot is not yet known. Missing %s", missing_str.c_str()); #pragma GCC diagnostic pop } @@ -1495,7 +1492,7 @@ void PlanningSceneMonitor::updateSceneWithCurrentState() { std::unique_lock ulock(scene_update_mutex_); last_update_time_ = last_robot_motion_time_ = current_state_monitor_->getCurrentStateTime(); - RCLCPP_DEBUG(logger_, "robot state update %f", fmod(last_robot_motion_time_.seconds(), 10.)); + RCLCPP_DEBUG(LOGGER, "robot state update %f", fmod(last_robot_motion_time_.seconds(), 10.)); current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst()); scene_->getCurrentStateNonConst().update(); // compute all transforms } @@ -1505,7 +1502,7 @@ void PlanningSceneMonitor::updateSceneWithCurrentState() { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(logger_, steady_clock, 1000, + RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000, "State monitor is not active. Unable to set the planning scene state"); #pragma GCC diagnostic pop } @@ -1527,7 +1524,7 @@ void PlanningSceneMonitor::clearUpdateCallbacks() void PlanningSceneMonitor::setPlanningScenePublishingFrequency(double hz) { publish_planning_scene_frequency_ = hz; - RCLCPP_DEBUG(logger_, "Maximum frequency for publishing a planning scene is now %lf Hz", + RCLCPP_DEBUG(LOGGER, "Maximum frequency for publishing a planning scene is now %lf Hz", publish_planning_scene_frequency_); } @@ -1549,7 +1546,7 @@ void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vectorhas_parameter(robot_description_ + "_planning.default_collision_operations")) { - RCLCPP_DEBUG(logger_, "No additional default collision operations specified"); + RCLCPP_DEBUG(LOGGER, "No additional default collision operations specified"); } else { - RCLCPP_DEBUG(logger_, "Reading additional default collision operations"); + RCLCPP_DEBUG(LOGGER, "Reading additional default collision operations"); // TODO: codebase wide refactoring for XmlRpc // XmlRpc::XmlRpcValue coll_ops; @@ -1604,13 +1601,13 @@ void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::Planni // if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray) // { - // RCLCPP_WARN(logger_, "default_collision_operations is not an array"); + // RCLCPP_WARN(LOGGER, "default_collision_operations is not an array"); // return; // } // if (coll_ops.size() == 0) // { - // RCLCPP_WARN(logger_, "No collision operations in default collision operations"); + // RCLCPP_WARN(LOGGER, "No collision operations in default collision operations"); // return; // } @@ -1619,7 +1616,7 @@ void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::Planni // if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || // !coll_ops[i].hasMember("operation")) // { - // RCLCPP_WARN(logger_, "All collision operations must have two objects and an operation"); + // RCLCPP_WARN(LOGGER, "All collision operations must have two objects and an operation"); // continue; // } // acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]), @@ -1655,7 +1652,7 @@ void PlanningSceneMonitor::configureDefaultPadding() // node_->get_parameter_or(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_, // std::map()); - RCLCPP_DEBUG_STREAM(logger_, "Loaded " << default_robot_link_padd_.size() << " default link paddings"); - RCLCPP_DEBUG_STREAM(logger_, "Loaded " << default_robot_link_scale_.size() << " default link scales"); + RCLCPP_DEBUG_STREAM(LOGGER, "Loaded " << default_robot_link_padd_.size() << " default link paddings"); + RCLCPP_DEBUG_STREAM(LOGGER, "Loaded " << default_robot_link_scale_.size() << " default link scales"); } } // namespace planning_scene_monitor 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..b3e1bd21cb 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -39,7 +39,8 @@ #include #include #include -#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_scene_monitor.trajectory_monitor"); planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency) @@ -55,7 +56,6 @@ 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")) { setSamplingFrequency(sampling_frequency); } @@ -72,11 +72,11 @@ void planning_scene_monitor::TrajectoryMonitor::setSamplingFrequency(double samp if (sampling_frequency <= std::numeric_limits::epsilon()) { - RCLCPP_ERROR(logger_, "The sampling frequency for trajectory states should be positive"); + RCLCPP_ERROR(LOGGER, "The sampling frequency for trajectory states should be positive"); } else { - RCLCPP_DEBUG(logger_, "Setting trajectory sampling frequency to %.1f", sampling_frequency); + RCLCPP_DEBUG(LOGGER, "Setting trajectory sampling frequency to %.1f", sampling_frequency); } sampling_frequency_ = sampling_frequency; } @@ -91,7 +91,7 @@ void planning_scene_monitor::TrajectoryMonitor::startTrajectoryMonitor() if (sampling_frequency_ > std::numeric_limits::epsilon() && !record_states_thread_) { record_states_thread_ = std::make_unique([this] { recordStates(); }); - RCLCPP_DEBUG(logger_, "Started trajectory monitor"); + RCLCPP_DEBUG(LOGGER, "Started trajectory monitor"); } } @@ -102,7 +102,7 @@ void planning_scene_monitor::TrajectoryMonitor::stopTrajectoryMonitor() std::unique_ptr copy; copy.swap(record_states_thread_); copy->join(); - RCLCPP_DEBUG(logger_, "Stopped trajectory monitor"); + RCLCPP_DEBUG(LOGGER, "Stopped trajectory monitor"); } } 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..debae970c5 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include @@ -56,10 +55,11 @@ namespace rdf_loader { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_rdf_loader.rdf_loader"); 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 +77,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(LOGGER, "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 +94,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(LOGGER, "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(LOGGER, "Unable to parse SRDF"); return false; } @@ -122,20 +122,20 @@ bool RDFLoader::loadFileToString(std::string& buffer, const std::string& path) { if (path.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "Path is empty"); + RCLCPP_ERROR(LOGGER, "Path is empty"); return false; } if (!std::filesystem::exists(path)) { - RCLCPP_ERROR(moveit::getLogger(), "File does not exist"); + RCLCPP_ERROR(LOGGER, "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(LOGGER, "Unable to load path"); return false; } @@ -155,13 +155,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(LOGGER, "Path is empty"); return false; } if (!std::filesystem::exists(path)) { - RCLCPP_ERROR(moveit::getLogger(), "File does not exist"); + RCLCPP_ERROR(LOGGER, "File does not exist"); return false; } @@ -177,7 +177,7 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa #endif if (!pipe) { - RCLCPP_ERROR(moveit::getLogger(), "Unable to load path"); + RCLCPP_ERROR(LOGGER, "Unable to load path"); return false; } @@ -217,7 +217,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(LOGGER, "ament_index_cpp: %s", e.what()); return false; } diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index 8b7c54220d..ba2e447354 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -133,6 +133,5 @@ class RobotModelLoader rdf_loader::RDFLoaderPtr rdf_loader_; kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_; const rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; }; } // namespace robot_model_loader 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..2c2e04bc04 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 @@ -42,22 +42,21 @@ #include #include #include -#include namespace robot_model_loader { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.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) { Options opt(robot_description); opt.load_kinematics_solvers = load_kinematics_solvers; configure(opt); } -RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const Options& opt) - : node_(node), logger_(moveit::makeChildLogger("robot_model_loader")) +RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const Options& opt) : node_(node) { configure(opt); } @@ -76,23 +75,23 @@ RobotModelLoader::~RobotModelLoader() namespace { -bool canSpecifyPosition(const moveit::core::JointModel* jmodel, const unsigned int index, const rclcpp::Logger& logger) +bool canSpecifyPosition(const moveit::core::JointModel* jmodel, const unsigned int index) { bool ok = false; if (jmodel->getType() == moveit::core::JointModel::PLANAR && index == 2) { - RCLCPP_ERROR(logger, "Cannot specify position limits for orientation of planar joint '%s'", + RCLCPP_ERROR(LOGGER, "Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str()); } else if (jmodel->getType() == moveit::core::JointModel::FLOATING && index > 2) { - RCLCPP_ERROR(logger, "Cannot specify position limits for orientation of floating joint '%s'", + RCLCPP_ERROR(LOGGER, "Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str()); } else if (jmodel->getType() == moveit::core::JointModel::REVOLUTE && static_cast(jmodel)->isContinuous()) { - RCLCPP_ERROR(logger, "Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str()); } else { @@ -143,7 +142,7 @@ void RobotModelLoader::configure(const Options& opt) double max_position; if (node_->get_parameter(param_name, max_position)) { - if (canSpecifyPosition(joint_model, joint_id, logger_)) + if (canSpecifyPosition(joint_model, joint_id)) { joint_limit[joint_id].has_position_limits = true; joint_limit[joint_id].max_position = max_position; @@ -158,7 +157,7 @@ void RobotModelLoader::configure(const Options& opt) double min_position; if (node_->get_parameter(param_name, min_position)) { - if (canSpecifyPosition(joint_model, joint_id, logger_)) + if (canSpecifyPosition(joint_model, joint_id)) { joint_limit[joint_id].has_position_limits = true; joint_limit[joint_id].min_position = min_position; @@ -203,7 +202,7 @@ void RobotModelLoader::configure(const Options& opt) if (!node_->get_parameter(param_name, joint_limit[joint_id].max_velocity)) { - RCLCPP_ERROR(logger_, "Specified a velocity limit for joint: %s but did not set a max velocity", + RCLCPP_ERROR(LOGGER, "Specified a velocity limit for joint: %s but did not set a max velocity", joint_limit[joint_id].joint_name.c_str()); } } @@ -218,7 +217,7 @@ void RobotModelLoader::configure(const Options& opt) if (!node_->get_parameter(param_name, joint_limit[joint_id].max_acceleration)) { - RCLCPP_ERROR(logger_, "Specified an acceleration limit for joint: %s but did not set a max acceleration", + RCLCPP_ERROR(LOGGER, "Specified an acceleration limit for joint: %s but did not set a max acceleration", joint_limit[joint_id].joint_name.c_str()); } } @@ -233,14 +232,14 @@ void RobotModelLoader::configure(const Options& opt) if (!node_->get_parameter(param_name, joint_limit[joint_id].max_jerk)) { - RCLCPP_ERROR(logger_, "Specified a jerk limit for joint: %s but did not set a max jerk", + RCLCPP_ERROR(LOGGER, "Specified a jerk limit for joint: %s but did not set a max jerk", joint_limit[joint_id].joint_name.c_str()); } } } catch (const rclcpp::ParameterTypeException& e) { - RCLCPP_ERROR_STREAM(logger_, "When getting the parameter " << param_name.c_str() << ": " << e.what()); + RCLCPP_ERROR_STREAM(LOGGER, "When getting the parameter " << param_name.c_str() << ": " << e.what()); } } joint_model->setVariableBounds(joint_limit); @@ -250,7 +249,7 @@ void RobotModelLoader::configure(const Options& opt) if (model_ && opt.load_kinematics_solvers) loadKinematicsSolvers(); - RCLCPP_DEBUG(logger_, "Loaded kinematic model in %f seconds", (clock.now() - start).seconds()); + RCLCPP_DEBUG(node_->get_logger(), "Loaded kinematic model in %f seconds", (clock.now() - start).seconds()); } void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader) @@ -272,9 +271,9 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin const std::vector& groups = kinematics_loader_->getKnownGroups(); std::stringstream ss; std::copy(groups.begin(), groups.end(), std::ostream_iterator(ss, " ")); - RCLCPP_DEBUG(logger_, "Loaded information about the following groups: '%s' ", ss.str().c_str()); + RCLCPP_DEBUG(LOGGER, "Loaded information about the following groups: '%s' ", ss.str().c_str()); if (groups.empty() && !model_->getJointModelGroups().empty()) - RCLCPP_WARN(logger_, "No kinematics plugins defined. Fill and load kinematics.yaml!"); + RCLCPP_WARN(LOGGER, "No kinematics plugins defined. Fill and load kinematics.yaml!"); std::map imap; for (const std::string& group : groups) @@ -296,13 +295,13 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin else { const auto& s = *solver; // avoid clang-tidy's -Wpotentially-evaluated-expression - RCLCPP_ERROR(logger_, "Kinematics solver %s does not support joint group %s. Error: %s", typeid(s).name(), + RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s", typeid(s).name(), group.c_str(), error_msg.c_str()); } } else { - RCLCPP_ERROR(logger_, "Kinematics solver could not be instantiated for joint group %s.", group.c_str()); + RCLCPP_ERROR(LOGGER, "Kinematics solver could not be instantiated for joint group %s.", group.c_str()); } } model_->setKinematicsAllocators(imap); diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index b76aaf6fb3..e1ae1af1b1 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -278,7 +278,6 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager const std::string name_ = "trajectory_execution_manager"; rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; rclcpp::Node::SharedPtr controller_mgr_node_; std::shared_ptr private_executor_; std::thread private_executor_thread_; 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..b4b89168ff 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 @@ -38,10 +38,10 @@ #include #include #include -#include namespace trajectory_execution_manager { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.trajectory_execution_manager"); const std::string TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC = "trajectory_execution_event"; @@ -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), robot_model_(robot_model), csm_(csm) { if (!node_->get_parameter("moveit_manage_controllers", manage_controllers_)) manage_controllers_ = false; @@ -66,11 +66,7 @@ TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::Share const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers) - : node_(node) - , logger_(moveit::makeChildLogger("trajectory_execution_manager")) - , robot_model_(robot_model) - , csm_(csm) - , manage_controllers_(manage_controllers) + : node_(node), robot_model_(robot_model), csm_(csm), manage_controllers_(manage_controllers) { initialize(); } @@ -110,7 +106,7 @@ void TrajectoryExecutionManager::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL_STREAM(logger_, "Exception while creating controller manager plugin loader: " << ex.what()); + RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating controller manager plugin loader: " << ex.what()); return; } @@ -124,16 +120,16 @@ void TrajectoryExecutionManager::initialize() if (classes.size() == 1) { controller = classes[0]; - RCLCPP_WARN(logger_, + RCLCPP_WARN(LOGGER, "Parameter '~moveit_controller_manager' is not specified but only one " "matching plugin was found: '%s'. Using that one.", controller.c_str()); } else { - RCLCPP_FATAL(logger_, "Parameter '~moveit_controller_manager' not specified. This is needed to " - "identify the plugin to use for interacting with controllers. No paths can " - "be executed."); + RCLCPP_FATAL(LOGGER, "Parameter '~moveit_controller_manager' not specified. This is needed to " + "identify the plugin to use for interacting with controllers. No paths can " + "be executed."); return; } } @@ -141,13 +137,13 @@ void TrajectoryExecutionManager::initialize() // Deprecation warnings, October 2022 if (controller == "moveit_ros_control_interface/MoveItControllerManager") { - RCLCPP_WARN(logger_, "moveit_ros_control_interface/MoveItControllerManager is deprecated. Replace with " - "`moveit_ros_control_interface/Ros2ControlManager.`"); + RCLCPP_WARN(LOGGER, "moveit_ros_control_interface/MoveItControllerManager is deprecated. Replace with " + "`moveit_ros_control_interface/Ros2ControlManager.`"); } if (controller == "moveit_ros_control_interface/MoveItMultiControllerManager") { - RCLCPP_WARN(logger_, "moveit_ros_control_interface/MoveItMultiControllerManager is deprecated. Replace with " - "`moveit_ros_control_interface/Ros2ControlMultiManager.`"); + RCLCPP_WARN(LOGGER, "moveit_ros_control_interface/MoveItMultiControllerManager is deprecated. Replace with " + "`moveit_ros_control_interface/Ros2ControlMultiManager.`"); } if (!controller.empty()) @@ -176,7 +172,7 @@ void TrajectoryExecutionManager::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL_STREAM(logger_, "Exception while loading controller manager '" << controller << "': " << ex.what()); + RCLCPP_FATAL_STREAM(LOGGER, "Exception while loading controller manager '" << controller << "': " << ex.what()); } } } @@ -203,11 +199,11 @@ void TrajectoryExecutionManager::initialize() if (manage_controllers_) { - RCLCPP_INFO(logger_, "Trajectory execution is managing controllers"); + RCLCPP_INFO(LOGGER, "Trajectory execution is managing controllers"); } else { - RCLCPP_INFO(logger_, "Trajectory execution is not managing controllers"); + RCLCPP_INFO(LOGGER, "Trajectory execution is not managing controllers"); } auto controller_mgr_parameter_set_callback = [this](const std::vector& parameters) { @@ -298,13 +294,13 @@ void TrajectoryExecutionManager::processEvent(const std::string& event) } else { - RCLCPP_WARN_STREAM(logger_, "Unknown event type: '" << event << '\''); + RCLCPP_WARN_STREAM(LOGGER, "Unknown event type: '" << event << '\''); } } void TrajectoryExecutionManager::receiveEvent(const std_msgs::msg::String::ConstSharedPtr& event) { - RCLCPP_INFO_STREAM(logger_, "Received event '" << event->data << '\''); + RCLCPP_INFO_STREAM(LOGGER, "Received event '" << event->data << '\''); processEvent(event->data); } @@ -346,7 +342,7 @@ bool TrajectoryExecutionManager::push(const moveit_msgs::msg::RobotTrajectory& t { if (!execution_complete_) { - RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed"); + RCLCPP_ERROR(LOGGER, "Cannot push a new trajectory while another is being executed"); return false; } @@ -363,7 +359,7 @@ bool TrajectoryExecutionManager::push(const moveit_msgs::msg::RobotTrajectory& t // TODO: Provide message serialization // for (const moveit_msgs::msg::RobotTrajectory& trajectory_part : context->trajectory_parts_) // ss << trajectory_part << '\n'; - RCLCPP_INFO_STREAM(logger_, ss.str()); + RCLCPP_INFO_STREAM(LOGGER, ss.str()); } trajectories_.push_back(context); return true; @@ -428,7 +424,7 @@ void TrajectoryExecutionManager::reloadControllerInformation() } else { - RCLCPP_ERROR(logger_, "Failed to reload controllers: `controller_manager_` does not exist."); + RCLCPP_ERROR(LOGGER, "Failed to reload controllers: `controller_manager_` does not exist."); } } @@ -441,7 +437,7 @@ void TrajectoryExecutionManager::updateControllerState(const std::string& contro } else { - RCLCPP_ERROR(logger_, "Controller '%s' is not known.", controller.c_str()); + RCLCPP_ERROR(LOGGER, "Controller '%s' is not known.", controller.c_str()); } } @@ -452,13 +448,13 @@ void TrajectoryExecutionManager::updateControllerState(ControllerInformation& ci if (controller_manager_) { if (verbose_) - RCLCPP_INFO(logger_, "Updating information for controller '%s'.", ci.name_.c_str()); + RCLCPP_INFO(LOGGER, "Updating information for controller '%s'.", ci.name_.c_str()); ci.state_ = controller_manager_->getControllerState(ci.name_); ci.last_update_ = node_->now(); } } else if (verbose_) - RCLCPP_INFO(logger_, "Information for controller '%s' is assumed to be up to date.", ci.name_.c_str()); + RCLCPP_INFO(LOGGER, "Information for controller '%s' is assumed to be up to date.", ci.name_.c_str()); } void TrajectoryExecutionManager::updateControllersState(const rclcpp::Duration& age) @@ -486,7 +482,7 @@ bool TrajectoryExecutionManager::checkControllerCombination(std::vector& ac sac << available_controller << ' '; for (const std::string& actuated_joint : actuated_joints) saj << actuated_joint << ' '; - RCLCPP_INFO(logger_, "Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.", + RCLCPP_INFO(LOGGER, "Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.", controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size()); } @@ -718,7 +714,7 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::Ro std::map::iterator it = known_controllers_.find(controllers[i]); if (it == known_controllers_.end()) { - RCLCPP_ERROR_STREAM(logger_, "Controller " << controllers[i] << " not found."); + RCLCPP_ERROR_STREAM(LOGGER, "Controller " << controllers[i] << " not found."); return false; } std::vector intersect_mdof; @@ -728,7 +724,7 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::Ro std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(), actuated_joints_single.end(), std::back_inserter(intersect_single)); if (intersect_mdof.empty() && intersect_single.empty()) - RCLCPP_WARN_STREAM(logger_, "No joints to be distributed for controller " << controllers[i]); + RCLCPP_WARN_STREAM(LOGGER, "No joints to be distributed for controller " << controllers[i]); { if (!intersect_mdof.empty()) { @@ -841,12 +837,12 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont if (allowed_start_tolerance_ == 0) // skip validation on this magic number return true; - RCLCPP_INFO(logger_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); + RCLCPP_INFO(LOGGER, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); moveit::core::RobotStatePtr current_state; if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState())) { - RCLCPP_WARN(logger_, "Failed to validate trajectory: couldn't receive full current joint state within 1s"); + RCLCPP_WARN(LOGGER, "Failed to validate trajectory: couldn't receive full current joint state within 1s"); return false; } @@ -859,7 +855,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont const std::vector& joint_names = trajectory.joint_trajectory.joint_names; if (positions.size() != joint_names.size()) { - RCLCPP_ERROR(logger_, "Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(), positions.size()); + RCLCPP_ERROR(LOGGER, "Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(), positions.size()); return false; } @@ -868,7 +864,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]); if (!jm) { - RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_names[i]); + RCLCPP_ERROR_STREAM(LOGGER, "Unknown joint in trajectory: " << joint_names[i]); return false; } @@ -879,7 +875,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont jm->enforcePositionBounds(&traj_position); if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_) { - RCLCPP_ERROR(logger_, + RCLCPP_ERROR(LOGGER, "\nInvalid Trajectory: start point deviates from current robot state more than %g" "\njoint '%s': expected: %g, current: %g", allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position); @@ -895,7 +891,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont const std::vector& joint_names = trajectory.multi_dof_joint_trajectory.joint_names; if (transforms.size() != joint_names.size()) { - RCLCPP_ERROR(logger_, "Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(), + RCLCPP_ERROR(LOGGER, "Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(), transforms.size()); return false; } @@ -905,7 +901,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]); if (!jm) { - RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_names[i]); + RCLCPP_ERROR_STREAM(LOGGER, "Unknown joint in trajectory: " << joint_names[i]); return false; } @@ -921,10 +917,10 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_) { - RCLCPP_ERROR_STREAM(logger_, "\nInvalid Trajectory: start point deviates from current robot state more than " - << allowed_start_tolerance_ << "\nmulti-dof joint '" << joint_names[i] - << "': pos delta: " << offset.transpose() - << " rot delta: " << rotation.angle()); + RCLCPP_ERROR_STREAM(LOGGER, "\nInvalid Trajectory: start point deviates from current robot state more than " + << allowed_start_tolerance_ << "\nmulti-dof joint '" << joint_names[i] + << "': pos delta: " << offset.transpose() + << " rot delta: " << rotation.angle()); return false; } } @@ -963,7 +959,7 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, if (actuated_joints.empty()) { - RCLCPP_WARN(logger_, "The trajectory to execute specifies no joints"); + RCLCPP_WARN(LOGGER, "The trajectory to execute specifies no joints"); return false; } @@ -1015,7 +1011,7 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, { if (known_controllers_.find(controller) == known_controllers_.end()) { - RCLCPP_ERROR(logger_, "Controller '%s' is not known", controller.c_str()); + RCLCPP_ERROR(LOGGER, "Controller '%s' is not known", controller.c_str()); return false; } } @@ -1029,7 +1025,7 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, std::stringstream ss; for (const std::string& actuated_joint : actuated_joints) ss << actuated_joint << ' '; - RCLCPP_ERROR(logger_, "Unable to identify any set of controllers that can actuate the specified joints: [ %s]", + RCLCPP_ERROR(LOGGER, "Unable to identify any set of controllers that can actuate the specified joints: [ %s]", ss.str().c_str()); std::stringstream ss2; @@ -1044,7 +1040,7 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, ss2 << " " << *ji << '\n'; } } - RCLCPP_ERROR(logger_, "Known controllers and their joints:\n%s", ss2.str().c_str()); + RCLCPP_ERROR(LOGGER, "Known controllers and their joints:\n%s", ss2.str().c_str()); return false; } @@ -1065,7 +1061,7 @@ void TrajectoryExecutionManager::stopExecutionInternal() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Caught %s when canceling execution.", ex.what()); + RCLCPP_ERROR(LOGGER, "Caught %s when canceling execution.", ex.what()); } } } @@ -1087,7 +1083,7 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time last_execution_status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; execution_state_mutex_.unlock(); - RCLCPP_INFO(logger_, "Stopped trajectory execution."); + RCLCPP_INFO(LOGGER, "Stopped trajectory execution."); // wait for the execution thread to finish std::scoped_lock lock(execution_thread_mutex_); @@ -1170,7 +1166,7 @@ void TrajectoryExecutionManager::clear() trajectories_.clear(); } else - RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed"); + RCLCPP_ERROR(LOGGER, "Cannot push a new trajectory while another is being executed"); } void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback, @@ -1185,7 +1181,7 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& return; } - RCLCPP_INFO(logger_, "Starting trajectory execution ..."); + RCLCPP_INFO(LOGGER, "Starting trajectory execution ..."); // assume everything will be OK last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; @@ -1208,7 +1204,7 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& if (last_execution_status_ == moveit_controller_manager::ExecutionStatus::SUCCEEDED) waitForRobotToStop(*trajectories_[i - 1]); - RCLCPP_INFO(logger_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str()); + RCLCPP_INFO(LOGGER, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str()); // notify whoever is waiting for the event of trajectory completion execution_state_mutex_.lock(); @@ -1255,15 +1251,14 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Caught %s when retrieving controller handle", ex.what()); + RCLCPP_ERROR(LOGGER, "Caught %s when retrieving controller handle", ex.what()); } if (!h) { active_handles_.clear(); current_context_ = -1; last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED; - RCLCPP_ERROR(logger_, "No controller handle for controller '%s'. Aborting.", - context.controllers_[i].c_str()); + RCLCPP_ERROR(LOGGER, "No controller handle for controller '%s'. Aborting.", context.controllers_[i].c_str()); return false; } active_handles_[i] = h; @@ -1278,7 +1273,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Caught %s when sending trajectory to controller", ex.what()); + RCLCPP_ERROR(LOGGER, "Caught %s when sending trajectory to controller", ex.what()); } if (!ok) { @@ -1290,13 +1285,13 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Caught %s when canceling execution", ex.what()); + RCLCPP_ERROR(LOGGER, "Caught %s when canceling execution", ex.what()); } } - RCLCPP_ERROR(logger_, "Failed to send trajectory part %zu of %zu to controller %s", i + 1, + RCLCPP_ERROR(LOGGER, "Failed to send trajectory part %zu of %zu to controller %s", i + 1, context.trajectory_parts_.size(), active_handles_[i]->getName().c_str()); if (i > 0) - RCLCPP_ERROR(logger_, "Cancelling previously sent trajectory parts"); + RCLCPP_ERROR(LOGGER, "Cancelling previously sent trajectory parts"); active_handles_.clear(); current_context_ = -1; last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED; @@ -1398,7 +1393,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) { if (!execution_complete_ && node_->now() - current_time > expected_trajectory_duration) { - RCLCPP_ERROR(logger_, + RCLCPP_ERROR(LOGGER, "Controller is taking too long to execute trajectory (the expected upper " "bound for the trajectory execution was %lf seconds). Stopping trajectory.", expected_trajectory_duration.seconds()); @@ -1424,8 +1419,8 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } else if (handle->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::SUCCEEDED) { - RCLCPP_WARN_STREAM(logger_, "Controller handle " << handle->getName() << " reports status " - << handle->getLastExecutionStatus().asString()); + RCLCPP_WARN_STREAM(LOGGER, "Controller handle " << handle->getName() << " reports status " + << handle->getLastExecutionStatus().asString()); last_execution_status_ = handle->getLastExecutionStatus(); result = false; } @@ -1446,7 +1441,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } else { - RCLCPP_ERROR(logger_, "Active status of required controllers can not be assured."); + RCLCPP_ERROR(LOGGER, "Active status of required controllers can not be assured."); last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED; return false; } @@ -1457,7 +1452,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon // skip waiting for convergence? if (allowed_start_tolerance_ == 0 || !wait_for_trajectory_completion_) { - RCLCPP_INFO(logger_, "Not waiting for trajectory completion"); + RCLCPP_INFO(LOGGER, "Not waiting for trajectory completion"); return true; } @@ -1474,7 +1469,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon { if (!csm_->waitForCurrentState(node_->now(), time_remaining) || !(cur_state = csm_->getCurrentState())) { - RCLCPP_WARN(logger_, "Failed to receive current joint state"); + RCLCPP_WARN(LOGGER, "Failed to receive current joint state"); return false; } cur_state->enforceBounds(); @@ -1605,12 +1600,12 @@ bool TrajectoryExecutionManager::ensureActiveControllers(const std::vectorsecond.state_.active_) { - RCLCPP_DEBUG_STREAM(logger_, "Need to activate " << controller); + RCLCPP_DEBUG_STREAM(LOGGER, "Need to activate " << controller); controllers_to_activate.push_back(controller); joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end()); for (const std::string& overlapping_controller : it->second.overlapping_controllers_) @@ -1624,7 +1619,7 @@ bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector diff; std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(), diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 38ec463c9e..ec50305b17 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -47,7 +47,6 @@ #include #include #include -#include #include #include @@ -944,7 +943,6 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface std::map > remembered_joint_values_; class MoveGroupInterfaceImpl; MoveGroupInterfaceImpl* impl_; - rclcpp::Logger logger_; }; } // namespace planning_interface } // namespace moveit 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..a69d9e2b1f 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 @@ -57,7 +57,6 @@ #include #include #include -#include #include #include @@ -75,6 +74,7 @@ const std::string MoveGroupInterface::ROBOT_DESCRIPTION = "robot_description"; // name of the robot description (a param name, so it can be changed externally) const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps +const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_interface"); namespace { @@ -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), 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 @@ -124,14 +124,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { std::string error = "Unable to construct robot model. Please make sure all needed information is on the " "parameter server."; - RCLCPP_FATAL_STREAM(logger_, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } if (!getRobotModel()->hasJointModelGroup(opt.group_name)) { std::string error = "Group '" + opt.group_name + "' was not found."; - RCLCPP_FATAL_STREAM(logger_, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } @@ -191,7 +191,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl rclcpp::names::append(opt_.move_group_namespace, move_group::CARTESIAN_PATH_SERVICE_NAME), qosDefault(), callback_group_); - RCLCPP_INFO_STREAM(logger_, "Ready to take commands for planning group " << opt.group_name << '.'); + RCLCPP_INFO_STREAM(LOGGER, "Ready to take commands for planning group " << opt.group_name << '.'); } ~MoveGroupInterfaceImpl() @@ -368,7 +368,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (target_value > 1.0) { - RCLCPP_WARN(logger_, "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value); + RCLCPP_WARN(rclcpp::get_logger("move_group_interface"), "Limiting max_%s (%.2f) to 1.0.", factor_name, + target_value); variable = 1.0; } else if (target_value <= 0.0) @@ -377,7 +378,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl fallback_value); if (target_value < 0.0) { - RCLCPP_WARN(logger_, "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable); + RCLCPP_WARN(rclcpp::get_logger("move_group_interface"), "max_%s < 0.0! Setting to default: %.2f.", factor_name, + variable); } } else @@ -464,7 +466,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - RCLCPP_ERROR(logger_, "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), + RCLCPP_ERROR(LOGGER, "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), getRobotModel()->getModelFrame().c_str()); return false; } @@ -515,7 +517,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; if (eef.empty()) { - RCLCPP_ERROR(logger_, "No end-effector to set the pose for"); + RCLCPP_ERROR(LOGGER, "No end-effector to set the pose for"); return false; } else @@ -549,7 +551,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const geometry_msgs::msg::PoseStamped UNKNOWN; - RCLCPP_ERROR(logger_, "Pose for end-effector '%s' not known.", eef.c_str()); + RCLCPP_ERROR(LOGGER, "Pose for end-effector '%s' not known.", eef.c_str()); return UNKNOWN; } @@ -566,7 +568,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const std::vector EMPTY; - RCLCPP_ERROR(logger_, "Poses for end-effector '%s' are not known.", eef.c_str()); + RCLCPP_ERROR(LOGGER, "Poses for end-effector '%s' are not known.", eef.c_str()); return EMPTY; } @@ -599,7 +601,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!current_state_monitor_) { - RCLCPP_ERROR(logger_, "Unable to monitor current robot state"); + RCLCPP_ERROR(LOGGER, "Unable to monitor current robot state"); return false; } @@ -615,7 +617,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!current_state_monitor_) { - RCLCPP_ERROR(logger_, "Unable to get current robot state"); + RCLCPP_ERROR(LOGGER, "Unable to get current robot state"); return false; } @@ -625,7 +627,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!current_state_monitor_->waitForCurrentState(node_->now(), wait_seconds)) { - RCLCPP_ERROR(logger_, "Failed to fetch current robot state"); + RCLCPP_ERROR(LOGGER, "Failed to fetch current robot state"); return false; } @@ -637,10 +639,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!move_action_client_ || !move_action_client_->action_server_is_ready()) { - RCLCPP_INFO_STREAM(logger_, "MoveGroup action client/server not ready"); + RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server not ready"); return moveit::core::MoveItErrorCode::FAILURE; } - RCLCPP_INFO_STREAM(logger_, "MoveGroup action client/server ready"); + RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server ready"); moveit_msgs::action::MoveGroup::Goal goal; constructGoal(goal); @@ -660,10 +662,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!goal_handle) { done = true; - RCLCPP_INFO(logger_, "Planning request rejected"); + RCLCPP_INFO(LOGGER, "Planning request rejected"); } else - RCLCPP_INFO(logger_, "Planning request accepted"); + RCLCPP_INFO(LOGGER, "Planning request accepted"); }; send_goal_opts.result_callback = [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { @@ -674,16 +676,16 @@ class MoveGroupInterface::MoveGroupInterfaceImpl switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(logger_, "Planning request complete!"); + RCLCPP_INFO(LOGGER, "Planning request complete!"); break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_INFO(logger_, "Planning request aborted"); + RCLCPP_INFO(LOGGER, "Planning request aborted"); return; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_INFO(logger_, "Planning request canceled"); + RCLCPP_INFO(LOGGER, "Planning request canceled"); return; default: - RCLCPP_INFO(logger_, "Planning request unknown result code"); + RCLCPP_INFO(LOGGER, "Planning request unknown result code"); return; } }; @@ -698,14 +700,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (code != rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_ERROR_STREAM(logger_, "MoveGroupInterface::plan() failed or timeout reached"); + RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::plan() failed or timeout reached"); return res->error_code; } plan.trajectory = res->planned_trajectory; plan.start_state = res->trajectory_start; plan.planning_time = res->planning_time; - RCLCPP_INFO(logger_, "time taken to generate plan: %g seconds", plan.planning_time); + RCLCPP_INFO(LOGGER, "time taken to generate plan: %g seconds", plan.planning_time); return res->error_code; } @@ -714,7 +716,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!move_action_client_ || !move_action_client_->action_server_is_ready()) { - RCLCPP_INFO_STREAM(logger_, "MoveGroup action client/server not ready"); + RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server not ready"); return moveit::core::MoveItErrorCode::FAILURE; } @@ -737,10 +739,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!goal_handle) { done = true; - RCLCPP_INFO(logger_, "Plan and Execute request rejected"); + RCLCPP_INFO(LOGGER, "Plan and Execute request rejected"); } else - RCLCPP_INFO(logger_, "Plan and Execute request accepted"); + RCLCPP_INFO(LOGGER, "Plan and Execute request accepted"); }; send_goal_opts.result_callback = [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { @@ -751,16 +753,16 @@ class MoveGroupInterface::MoveGroupInterfaceImpl switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(logger_, "Plan and Execute request complete!"); + RCLCPP_INFO(LOGGER, "Plan and Execute request complete!"); break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_INFO(logger_, "Plan and Execute request aborted"); + RCLCPP_INFO(LOGGER, "Plan and Execute request aborted"); return; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_INFO(logger_, "Plan and Execute request canceled"); + RCLCPP_INFO(LOGGER, "Plan and Execute request canceled"); return; default: - RCLCPP_INFO(logger_, "Plan and Execute request unknown result code"); + RCLCPP_INFO(LOGGER, "Plan and Execute request unknown result code"); return; } }; @@ -776,7 +778,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (code != rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_ERROR_STREAM(logger_, "MoveGroupInterface::move() failed or timeout reached"); + RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::move() failed or timeout reached"); } return res->error_code; } @@ -786,7 +788,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!execute_action_client_ || !execute_action_client_->action_server_is_ready()) { - RCLCPP_INFO_STREAM(logger_, "execute_action_client_ client/server not ready"); + RCLCPP_INFO_STREAM(LOGGER, "execute_action_client_ client/server not ready"); return moveit::core::MoveItErrorCode::FAILURE; } @@ -800,10 +802,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!goal_handle) { done = true; - RCLCPP_INFO(logger_, "Execute request rejected"); + RCLCPP_INFO(LOGGER, "Execute request rejected"); } else - RCLCPP_INFO(logger_, "Execute request accepted"); + RCLCPP_INFO(LOGGER, "Execute request accepted"); }; send_goal_opts.result_callback = [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { @@ -814,16 +816,16 @@ class MoveGroupInterface::MoveGroupInterfaceImpl switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(logger_, "Execute request success!"); + RCLCPP_INFO(LOGGER, "Execute request success!"); break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_INFO(logger_, "Execute request aborted"); + RCLCPP_INFO(LOGGER, "Execute request aborted"); return; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_INFO(logger_, "Execute request canceled"); + RCLCPP_INFO(LOGGER, "Execute request canceled"); return; default: - RCLCPP_INFO(logger_, "Execute request unknown result code"); + RCLCPP_INFO(LOGGER, "Execute request unknown result code"); return; } }; @@ -844,7 +846,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (code != rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_ERROR_STREAM(logger_, "MoveGroupInterface::execute() failed or timeout reached"); + RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::execute() failed or timeout reached"); } return res->error_code; } @@ -919,7 +921,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } if (l.empty()) { - RCLCPP_ERROR(logger_, "No known link to attach object '%s' to", object.c_str()); + RCLCPP_ERROR(LOGGER, "No known link to attach object '%s' to", object.c_str()); return false; } moveit_msgs::msg::AttachedCollisionObject aco; @@ -1063,7 +1065,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } else - RCLCPP_ERROR(logger_, "Unable to construct MotionPlanRequest representation"); + RCLCPP_ERROR(LOGGER, "Unable to construct MotionPlanRequest representation"); if (path_constraints_) request.path_constraints = *path_constraints_; @@ -1249,14 +1251,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } initializing_constraints_ = false; } Options opt_; rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_executor_; std::thread callback_thread_; @@ -1319,7 +1320,6 @@ 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")) { if (!rclcpp::ok()) throw std::runtime_error("ROS does not seem to be running"); @@ -1330,7 +1330,6 @@ 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")) { impl_ = new MoveGroupInterfaceImpl(node, opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); } @@ -1341,7 +1340,7 @@ 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_) { other.impl_ = nullptr; } @@ -1352,7 +1351,6 @@ MoveGroupInterface& MoveGroupInterface::operator=(MoveGroupInterface&& other) no { delete impl_; impl_ = other.impl_; - logger_ = other.logger_; remembered_joint_values_ = std::move(other.remembered_joint_values_); other.impl_ = nullptr; } @@ -1620,7 +1618,7 @@ std::map MoveGroupInterface::getNamedTargetValues(const std { if (!impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions)) { - RCLCPP_ERROR(logger_, "The requested named target '%s' does not exist, returning empty positions.", name.c_str()); + RCLCPP_ERROR(LOGGER, "The requested named target '%s' does not exist, returning empty positions.", name.c_str()); } } return positions; @@ -1640,7 +1638,7 @@ bool MoveGroupInterface::setNamedTarget(const std::string& name) impl_->setTargetType(JOINT); return true; } - RCLCPP_ERROR(logger_, "The requested named target '%s' does not exist", name.c_str()); + RCLCPP_ERROR(LOGGER, "The requested named target '%s' does not exist", name.c_str()); return false; } } @@ -1655,9 +1653,9 @@ bool MoveGroupInterface::setJointValueTarget(const std::vector& joint_va const auto n_group_joints = impl_->getJointModelGroup()->getVariableCount(); if (joint_values.size() != n_group_joints) { - RCLCPP_DEBUG_STREAM(logger_, "Provided joint value list has length " << joint_values.size() << " but group " - << impl_->getJointModelGroup()->getName() - << " has " << n_group_joints << " joints"); + RCLCPP_DEBUG_STREAM(LOGGER, "Provided joint value list has length " << joint_values.size() << " but group " + << impl_->getJointModelGroup()->getName() + << " has " << n_group_joints << " joints"); return false; } impl_->setTargetType(JOINT); @@ -1672,8 +1670,8 @@ bool MoveGroupInterface::setJointValueTarget(const std::map { if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end()) { - RCLCPP_ERROR_STREAM(logger_, "joint variable " << pair.first << " is not part of group " - << impl_->getJointModelGroup()->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "joint variable " << pair.first << " is not part of group " + << impl_->getJointModelGroup()->getName()); return false; } } @@ -1688,7 +1686,7 @@ bool MoveGroupInterface::setJointValueTarget(const std::vector& var { if (variable_names.size() != variable_values.size()) { - RCLCPP_ERROR_STREAM(logger_, "sizes of name and position arrays do not match"); + RCLCPP_ERROR_STREAM(LOGGER, "sizes of name and position arrays do not match"); return false; } const auto& allowed = impl_->getJointModelGroup()->getVariableNames(); @@ -1696,8 +1694,8 @@ bool MoveGroupInterface::setJointValueTarget(const std::vector& var { if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end()) { - RCLCPP_ERROR_STREAM(logger_, "joint variable " << variable_name << " is not part of group " - << impl_->getJointModelGroup()->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "joint variable " << variable_name << " is not part of group " + << impl_->getJointModelGroup()->getName()); return false; } } @@ -1730,7 +1728,7 @@ bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, cons return impl_->getTargetRobotState().satisfiesBounds(jm, impl_->getGoalJointTolerance()); } - RCLCPP_ERROR_STREAM(logger_, + RCLCPP_ERROR_STREAM(LOGGER, "joint " << joint_name << " is not part of group " << impl_->getJointModelGroup()->getName()); return false; } @@ -1878,7 +1876,7 @@ bool MoveGroupInterface::setPoseTargets(const std::vector MoveGroupInterface::getCurrentRPY(const std::string& end_eff const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; if (eef.empty()) { - RCLCPP_ERROR(logger_, "No end-effector specified"); + RCLCPP_ERROR(LOGGER, "No end-effector specified"); } else { @@ -2178,18 +2176,18 @@ void MoveGroupInterface::forgetJointValues(const std::string& name) void MoveGroupInterface::allowLooking(bool flag) { impl_->can_look_ = flag; - RCLCPP_DEBUG(logger_, "Looking around: %s", flag ? "yes" : "no"); + RCLCPP_DEBUG(LOGGER, "Looking around: %s", flag ? "yes" : "no"); } void MoveGroupInterface::setLookAroundAttempts(int32_t attempts) { if (attempts < 0) { - RCLCPP_ERROR(logger_, "Tried to set negative number of look-around attempts"); + RCLCPP_ERROR(LOGGER, "Tried to set negative number of look-around attempts"); } else { - RCLCPP_DEBUG_STREAM(logger_, "Look around attempts: " << attempts); + RCLCPP_DEBUG_STREAM(LOGGER, "Look around attempts: " << attempts); impl_->look_around_attempts_ = attempts; } } @@ -2197,18 +2195,18 @@ void MoveGroupInterface::setLookAroundAttempts(int32_t attempts) void MoveGroupInterface::allowReplanning(bool flag) { impl_->can_replan_ = flag; - RCLCPP_DEBUG(logger_, "Replanning: %s", flag ? "yes" : "no"); + RCLCPP_DEBUG(LOGGER, "Replanning: %s", flag ? "yes" : "no"); } void MoveGroupInterface::setReplanAttempts(int32_t attempts) { if (attempts < 0) { - RCLCPP_ERROR(logger_, "Tried to set negative number of replan attempts"); + RCLCPP_ERROR(LOGGER, "Tried to set negative number of replan attempts"); } else { - RCLCPP_DEBUG_STREAM(logger_, "Replan Attempts: " << attempts); + RCLCPP_DEBUG_STREAM(LOGGER, "Replan Attempts: " << attempts); impl_->replan_attempts_ = attempts; } } @@ -2217,11 +2215,11 @@ void MoveGroupInterface::setReplanDelay(double delay) { if (delay < 0.0) { - RCLCPP_ERROR(logger_, "Tried to set negative replan delay"); + RCLCPP_ERROR(LOGGER, "Tried to set negative replan delay"); } else { - RCLCPP_DEBUG_STREAM(logger_, "Replan Delay: " << delay); + RCLCPP_DEBUG_STREAM(LOGGER, "Replan Delay: " << delay); impl_->replan_delay_ = delay; } } 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..67da194b79 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 @@ -41,14 +41,12 @@ #include #include #include -#include - -using moveit::getLogger; namespace moveit { namespace planning_interface { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_scene_interface.planning_scene_interface"); class PlanningSceneInterface::PlanningSceneInterfaceImpl { @@ -60,7 +58,6 @@ 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()); 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 +112,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(LOGGER, "Could not call planning scene service to get object names"); return result; } response = res.get(); @@ -188,7 +185,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(LOGGER, "Could not call planning scene service to get object geometries"); return result; } response = res.get(); @@ -214,7 +211,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(LOGGER, "Could not call planning scene service to get attached object geometries"); return result; } response = res.get(); @@ -240,7 +237,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(LOGGER, "Failed to call ApplyPlanningScene service"); } response = res.get(); return response->success; @@ -292,8 +289,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl srv->wait_for_service(std::chrono::duration_cast(d)); if (!srv->service_is_ready()) { - RCLCPP_WARN_STREAM(getLogger(), - "service '" << srv->get_service_name() << "' not advertised yet. Continue waiting..."); + RCLCPP_WARN_STREAM(LOGGER, "service '" << srv->get_service_name() << "' not advertised yet. Continue waiting..."); srv->wait_for_service(); } } diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index 31bf726f21..0ae3ae2041 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -233,7 +232,6 @@ class RobotInteraction std::string topic_; rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; // options for doing IK KinematicOptionsMapPtr kinematic_options_map_; diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index 1a598e7e99..2422875cc4 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -46,7 +46,6 @@ #include #include #include -#include #include #include @@ -55,6 +54,7 @@ namespace robot_interaction { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.interaction_handler"); InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, const moveit::core::RobotState& initial_robot_state, @@ -406,14 +406,14 @@ bool InteractionHandler::transformFeedbackPose( } catch (tf2::TransformException& e) { - RCLCPP_ERROR(moveit::getLogger(), "Error transforming from frame '%s' to frame '%s'", - tpose.header.frame_id.c_str(), planning_frame_.c_str()); + RCLCPP_ERROR(LOGGER, "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(LOGGER, "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..ea03b44119 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -36,7 +36,8 @@ #include #include -#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.kinematic_options"); robot_interaction::KinematicOptions::KinematicOptions() : timeout_seconds_(0.0) // 0.0 = use default timeout { @@ -51,7 +52,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(LOGGER, "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..e85d54cbfd 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include @@ -56,6 +55,7 @@ namespace robot_interaction { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.robot_interaction"); static const double END_EFFECTOR_UNREACHABLE_COLOR[4] = { 1.0, 0.3, 0.3, 1.0 }; static const double END_EFFECTOR_REACHABLE_COLOR[4] = { 0.2, 1.0, 0.2, 1.0 }; @@ -63,12 +63,10 @@ const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interactio RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, const std::string& ns) - : robot_model_(robot_model) - , node_(node) - , logger_(moveit::makeChildLogger("moveit_ros_robot_interaction")) - , kinematic_options_map_(std::make_shared()) + : robot_model_(robot_model), kinematic_options_map_(std::make_shared()) { topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC; + node_ = node; int_marker_server_ = new interactive_markers::InteractiveMarkerServer(topic_, node_); // spin a thread that will process feedback events @@ -97,7 +95,7 @@ void RobotInteraction::decideActiveComponents(const std::string& group, Interact decideActiveJoints(group); if (!group.empty() && active_eef_.empty() && active_vj_.empty() && active_generic_.empty()) { - RCLCPP_INFO(logger_, + RCLCPP_INFO(LOGGER, "No active joints or end effectors found for group '%s'. " "Make sure that kinematics.yaml is loaded in this node's namespace.", group.c_str()); @@ -198,7 +196,7 @@ void RobotInteraction::decideActiveJoints(const std::string& group) if (group.empty()) return; - RCLCPP_DEBUG(logger_, "Deciding active joints for group '%s'", group.c_str()); + RCLCPP_DEBUG(LOGGER, "Deciding active joints for group '%s'", group.c_str()); const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF(); const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); @@ -284,14 +282,14 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera if (group.empty()) return; - RCLCPP_DEBUG(logger_, "Deciding active end-effectors for group '%s'", group.c_str()); + RCLCPP_DEBUG(LOGGER, "Deciding active end-effectors for group '%s'", group.c_str()); const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF(); const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg || !srdf) { - RCLCPP_WARN(logger_, "Unable to decide active end effector: no joint model group or no srdf model"); + RCLCPP_WARN(LOGGER, "Unable to decide active end effector: no joint model group or no srdf model"); return; } @@ -355,7 +353,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera // otherwise, we use the size of the parent_link eef.size = (eef.eef_group == eef.parent_group) ? computeLinkMarkerSize(eef.parent_link) : computeGroupMarkerSize(eef.eef_group); - RCLCPP_DEBUG(logger_, "Found active end-effector '%s', of scale %lf", eef.eef_group.c_str(), eef.size); + RCLCPP_DEBUG(LOGGER, "Found active end-effector '%s', of scale %lf", eef.eef_group.c_str(), eef.size); } // if there is only a single end effector marker, we can safely use a larger marker if (active_eef_.size() == 1) @@ -487,7 +485,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle im.name = getMarkerName(handler, active_generic_[i]); shown_markers_[im.name] = i; ims.push_back(im); - RCLCPP_DEBUG(logger_, "Publishing interactive marker %s (size = %lf)", im.name.c_str(), im.scale); + RCLCPP_DEBUG(LOGGER, "Publishing interactive marker %s (size = %lf)", im.name.c_str(), im.scale); } } @@ -532,7 +530,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle } ims.push_back(im); registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_eef_[i].parent_link); - RCLCPP_DEBUG(logger_, "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); + RCLCPP_DEBUG(LOGGER, "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); } for (std::size_t i = 0; i < active_vj_.size(); ++i) { @@ -560,7 +558,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle } ims.push_back(im); registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_vj_[i].connecting_link); - RCLCPP_DEBUG(logger_, "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); + RCLCPP_DEBUG(LOGGER, "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); } handlers_[handler->getName()] = handler; } @@ -741,7 +739,7 @@ void RobotInteraction::processInteractiveMarkerFeedback( std::map::const_iterator it = shown_markers_.find(feedback->marker_name); if (it == shown_markers_.end()) { - RCLCPP_ERROR(logger_, "Unknown marker name: '%s' (not published by RobotInteraction class)", + RCLCPP_ERROR(LOGGER, "Unknown marker name: '%s' (not published by RobotInteraction class)", feedback->marker_name.c_str()); return; } @@ -749,7 +747,7 @@ void RobotInteraction::processInteractiveMarkerFeedback( std::size_t u = feedback->marker_name.find_first_of('_'); if (u == std::string::npos || u < 4) { - RCLCPP_ERROR(logger_, "Invalid marker name: '%s'", feedback->marker_name.c_str()); + RCLCPP_ERROR(LOGGER, "Invalid marker name: '%s'", feedback->marker_name.c_str()); return; } @@ -770,12 +768,12 @@ void RobotInteraction::processingThread() { auto feedback = feedback_map_.begin()->second; feedback_map_.erase(feedback_map_.begin()); - RCLCPP_DEBUG(logger_, "Processing feedback from map for marker [%s]", feedback->marker_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Processing feedback from map for marker [%s]", feedback->marker_name.c_str()); std::map::const_iterator it = shown_markers_.find(feedback->marker_name); if (it == shown_markers_.end()) { - RCLCPP_ERROR(logger_, + RCLCPP_ERROR(LOGGER, "Unknown marker name: '%s' (not published by RobotInteraction class) " "(should never have ended up in the feedback_map!)", feedback->marker_name.c_str()); @@ -784,7 +782,7 @@ void RobotInteraction::processingThread() std::size_t u = feedback->marker_name.find_first_of('_'); if (u == std::string::npos || u < 4) { - RCLCPP_ERROR(logger_, "Invalid marker name: '%s' (should never have ended up in the feedback_map!)", + RCLCPP_ERROR(LOGGER, "Invalid marker name: '%s' (should never have ended up in the feedback_map!)", feedback->marker_name.c_str()); continue; } @@ -793,7 +791,7 @@ void RobotInteraction::processingThread() std::map::const_iterator jt = handlers_.find(handler_name); if (jt == handlers_.end()) { - RCLCPP_ERROR(logger_, "Interactive Marker Handler '%s' is not known.", handler_name.c_str()); + RCLCPP_ERROR(LOGGER, "Interactive Marker Handler '%s' is not known.", handler_name.c_str()); continue; } @@ -812,7 +810,7 @@ void RobotInteraction::processingThread() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Exception caught while handling end-effector update: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught while handling end-effector update: %s", ex.what()); } marker_access_lock_.lock(); } @@ -828,7 +826,7 @@ void RobotInteraction::processingThread() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Exception caught while handling joint update: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught while handling joint update: %s", ex.what()); } marker_access_lock_.lock(); } @@ -843,19 +841,19 @@ void RobotInteraction::processingThread() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Exception caught while handling joint update: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught while handling joint update: %s", ex.what()); } marker_access_lock_.lock(); } else { - RCLCPP_ERROR(logger_, "Unknown marker class ('%s') for marker '%s'", marker_class.c_str(), + RCLCPP_ERROR(LOGGER, "Unknown marker class ('%s') for marker '%s'", marker_class.c_str(), feedback->marker_name.c_str()); } } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Exception caught while processing event: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught while processing event: %s", ex.what()); } } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 6288c349ab..20190830d0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -58,7 +58,6 @@ #include #include #include -#include #endif #include @@ -326,7 +325,6 @@ private Q_SLOTS: void setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list); rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr planning_scene_publisher_; rclcpp::Publisher::SharedPtr planning_scene_world_publisher_; 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..7dd4ce0ee6 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 @@ -69,10 +69,10 @@ #include "ui_motion_planning_rviz_plugin_frame.h" #include -#include namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_display"); // ****************************************************************************************** // Base class constructor @@ -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(LOGGER, "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..af37922f46 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 @@ -41,7 +41,6 @@ #include #include #include -#include #include @@ -61,20 +60,16 @@ namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame"); MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_common::DisplayContext* context, QWidget* parent) - : QWidget(parent) - , planning_display_(pdisplay) - , context_(context) - , ui_(new Ui::MotionPlanningUI()) - , logger_(moveit::makeChildLogger("motion_planning_frame")) - , first_time_(true) + : QWidget(parent), planning_display_(pdisplay), context_(context), ui_(new Ui::MotionPlanningUI()), first_time_(true) { auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); if (!ros_node_abstraction) { - RCLCPP_INFO(logger_, "Unable to lock weak_ptr from DisplayContext in MotionPlanningFrame constructor"); + RCLCPP_INFO(LOGGER, "Unable to lock weak_ptr from DisplayContext in MotionPlanningFrame constructor"); return; } node_ = ros_node_abstraction->get_raw_node(); @@ -216,7 +211,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c { if (!object_recognition_client_->wait_for_action_server(std::chrono::seconds(3))) { - RCLCPP_ERROR(logger_, "Action server: %s not available", OBJECT_RECOGNITION_ACTION.c_str()); + RCLCPP_ERROR(LOGGER, "Action server: %s not available", OBJECT_RECOGNITION_ACTION.c_str()); object_recognition_client_.reset(); } } @@ -238,7 +233,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c // } // catch (std::exception& ex) // { - // RCLCPP_ERROR(logger_, "Failed to get semantic world: %s", ex.what()); + // RCLCPP_ERROR(LOGGER, "Failed to get semantic world: %s", ex.what()); // } } @@ -381,10 +376,10 @@ void MotionPlanningFrame::changePlanningGroupHelper() if (!group.empty() && robot_model) { - RCLCPP_INFO(logger_, "group %s", group.c_str()); + RCLCPP_INFO(LOGGER, "group %s", group.c_str()); if (move_group_ && move_group_->getName() == group) return; - RCLCPP_INFO(logger_, "Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(), + RCLCPP_INFO(LOGGER, "Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(), planning_display_->getMoveGroupNS().c_str()); moveit::planning_interface::MoveGroupInterface::Options opt( group, moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION, planning_display_->getMoveGroupNS()); @@ -406,7 +401,7 @@ void MotionPlanningFrame::changePlanningGroupHelper() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } planning_display_->addMainLoopJob([&view = *ui_->planner_param_treeview, this] { view.setMoveGroup(move_group_); }); if (move_group_) @@ -603,7 +598,7 @@ void MotionPlanningFrame::enable() const std::string new_ns = planning_display_->getMoveGroupNS(); if (node_->get_namespace() != new_ns) { - RCLCPP_INFO(logger_, "MoveGroup namespace changed: %s -> %s. Reloading params.", node_->get_namespace(), + RCLCPP_INFO(LOGGER, "MoveGroup namespace changed: %s -> %s. Reloading params.", node_->get_namespace(), new_ns.c_str()); initFromMoveGroupNS(); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index 497f2655fd..f62ad22343 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -49,6 +49,7 @@ namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_context"); void MotionPlanningFrame::databaseConnectButtonClicked() { @@ -111,7 +112,7 @@ void MotionPlanningFrame::resetDbButtonClicked() void MotionPlanningFrame::computeDatabaseConnectButtonClicked() { - RCLCPP_INFO(logger_, "Connect to database: {host: %s, port: %d}", ui_->database_host->text().toStdString().c_str(), + RCLCPP_INFO(LOGGER, "Connect to database: {host: %s, port: %d}", ui_->database_host->text().toStdString().c_str(), ui_->database_port->value()); if (planning_scene_storage_) { @@ -142,7 +143,7 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClicked() catch (std::exception& ex) { planning_display_->addMainLoopJob([this] { computeDatabaseConnectButtonClickedHelper(3); }); - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); return; } planning_display_->addMainLoopJob([this] { computeDatabaseConnectButtonClickedHelper(4); }); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 3157891ec2..3614f102cc 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -45,6 +45,7 @@ namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_joints_widget"); JMGItemModel::JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent) : QAbstractTableModel(parent), robot_state_(robot_state), jmg_(nullptr) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index 5c20b48776..d542387604 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -47,6 +47,7 @@ namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_manipulation"); /////////////// Object Detection /////////////////////// void MotionPlanningFrame::detectObjectsButtonClicked() @@ -103,7 +104,7 @@ void MotionPlanningFrame::processDetectedObjects() rclcpp::sleep_for(std::chrono::milliseconds(500)); } - RCLCPP_DEBUG(logger_, "Found %d objects", static_cast(object_ids.size())); + RCLCPP_DEBUG(LOGGER, "Found %d objects", static_cast(object_ids.size())); updateDetectedObjectsList(object_ids); } @@ -112,7 +113,7 @@ void MotionPlanningFrame::selectedDetectedObjectChanged() QList sel = ui_->detected_objects_list->selectedItems(); if (sel.empty()) { - RCLCPP_INFO(logger_, "No objects to select"); + RCLCPP_INFO(LOGGER, "No objects to select"); return; } planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); @@ -143,7 +144,7 @@ void MotionPlanningFrame::triggerObjectDetection() node_, OBJECT_RECOGNITION_ACTION); if (!object_recognition_client_->wait_for_action_server(std::chrono::seconds(3))) { - RCLCPP_ERROR(logger_, "Object recognition action server not responsive"); + RCLCPP_ERROR(LOGGER, "Object recognition action server not responsive"); return; } } @@ -153,7 +154,7 @@ void MotionPlanningFrame::triggerObjectDetection() goal_handle_future.wait(); if (goal_handle_future.get()->get_status() != rclcpp_action::GoalStatus::STATUS_SUCCEEDED) { - RCLCPP_ERROR(logger_, "ObjectRecognition client: send goal call failed"); + RCLCPP_ERROR(LOGGER, "ObjectRecognition client: send goal call failed"); return; } } @@ -191,7 +192,7 @@ void MotionPlanningFrame::updateDetectedObjectsList(const std::vectoraddBackgroundJob([this] { publishTables(); }, "publish tables"); } @@ -208,7 +209,7 @@ void MotionPlanningFrame::selectedSupportSurfaceChanged() QList sel = ui_->support_surfaces_list->selectedItems(); if (sel.empty()) { - RCLCPP_INFO(logger_, "No tables to select"); + RCLCPP_INFO(LOGGER, "No tables to select"); return; } planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); @@ -240,7 +241,7 @@ void MotionPlanningFrame::updateSupportSurfacesList() // std::vector support_ids = semantic_world_->getTableNamesInROI(min_x, min_y, min_z, max_x, max_y, // max_z); std::vector support_ids; - RCLCPP_INFO(logger_, "%d Tables in collision world", static_cast(support_ids.size())); + RCLCPP_INFO(LOGGER, "%d Tables in collision world", static_cast(support_ids.size())); ui_->support_surfaces_list->setUpdatesEnabled(false); bool old_state = ui_->support_surfaces_list->blockSignals(true); @@ -264,14 +265,14 @@ void MotionPlanningFrame::updateSupportSurfacesList() /////////////////////////////// Pick & Place ///////////////////////////////// void MotionPlanningFrame::pickObjectButtonClicked() { - RCLCPP_WARN_STREAM(logger_, "Pick & Place capability isn't supported yet"); + RCLCPP_WARN_STREAM(LOGGER, "Pick & Place capability isn't supported yet"); // QList sel = ui_->detected_objects_list->selectedItems(); // QList sel_table = ui_->support_surfaces_list->selectedItems(); // // std::string group_name = planning_display_->getCurrentPlanningGroup(); // if (sel.empty()) // { - // RCLCPP_INFO(logger_, "No objects to pick"); + // RCLCPP_INFO(LOGGER, "No objects to pick"); // return; // } // pick_object_name_[group_name] = sel[0]->text().toStdString(); @@ -287,7 +288,7 @@ void MotionPlanningFrame::pickObjectButtonClicked() // { // geometry_msgs::msg::Pose // object_pose(tf2::toMsg(ps->getWorld()->getTransform(pick_object_name_[group_name]))); - // RCLCPP_DEBUG(logger_, "Finding current table for object: " << pick_object_name_[group_name]); + // RCLCPP_DEBUG(LOGGER, "Finding current table for object: " << pick_object_name_[group_name]); // support_surface_name_ = semantic_world_->findObjectTable(object_pose); // } // else @@ -296,7 +297,7 @@ void MotionPlanningFrame::pickObjectButtonClicked() // else // support_surface_name_.clear(); // } - // RCLCPP_INFO(logger_, "Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(), + // RCLCPP_INFO(LOGGER, "Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(), // support_surface_name_.c_str()); // planning_display_->addBackgroundJob([this] { pickObject(); }, "pick"); } @@ -313,7 +314,7 @@ void MotionPlanningFrame::placeObjectButtonClicked() else { support_surface_name_.clear(); - RCLCPP_ERROR(logger_, "Need to specify table to place object on"); + RCLCPP_ERROR(LOGGER, "Need to specify table to place object on"); return; } @@ -324,7 +325,7 @@ void MotionPlanningFrame::placeObjectButtonClicked() const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (!ps) { - RCLCPP_ERROR(logger_, "No planning scene"); + RCLCPP_ERROR(LOGGER, "No planning scene"); return; } const moveit::core::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name); @@ -333,7 +334,7 @@ void MotionPlanningFrame::placeObjectButtonClicked() if (attached_bodies.empty()) { - RCLCPP_ERROR(logger_, "No bodies to place"); + RCLCPP_ERROR(LOGGER, "No bodies to place"); return; } @@ -356,7 +357,7 @@ void MotionPlanningFrame::placeObjectButtonClicked() // ui_->pick_button->setEnabled(false); // if (pick_object_name_.find(group_name) == pick_object_name_.end()) // { -// RCLCPP_ERROR(logger_, "No pick object set for this group"); +// RCLCPP_ERROR(LOGGER, "No pick object set for this group"); // return; // } // if (!support_surface_name_.empty()) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 4d473bbf11..2e7fab9b89 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -72,6 +72,7 @@ QString subframePosesToQstring(const moveit::core::FixedTransformsMap& subframes namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_objects"); void MotionPlanningFrame::shapesComboBoxChanged(const QString& /*text*/) { @@ -454,7 +455,7 @@ void MotionPlanningFrame::imProcessFeedback(visualization_msgs::msg::Interactive { if (!planning_display_->getPlanningSceneRO()->knowsFrameTransform(feedback.header.frame_id)) { - RCLCPP_ERROR_STREAM(logger_, + RCLCPP_ERROR_STREAM(LOGGER, "Frame `" << feedback.header.frame_id << "` unknown doesn't exists in the planning scene"); } Eigen::Isometry3d fixed_frame_t_scene_marker; @@ -519,7 +520,7 @@ void MotionPlanningFrame::copySelectedCollisionObject() name += std::to_string(n); } ps->getWorldNonConst()->addToObject(name, obj->shapes_, obj->shape_poses_); - RCLCPP_DEBUG(logger_, "Copied collision object to '%s'", name.c_str()); + RCLCPP_DEBUG(LOGGER, "Copied collision object to '%s'", name.c_str()); } setLocalSceneEdited(); planning_display_->addMainLoopJob([this] { populateCollisionObjectsList(); }); @@ -538,7 +539,7 @@ void MotionPlanningFrame::computeSaveSceneButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } planning_display_->addMainLoopJob([this] { populatePlanningSceneTreeView(); }); @@ -559,7 +560,7 @@ void MotionPlanningFrame::computeSaveQueryButtonClicked(const std::string& scene } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } planning_display_->addMainLoopJob([this] { populatePlanningSceneTreeView(); }); @@ -583,7 +584,7 @@ void MotionPlanningFrame::computeDeleteSceneButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } } else @@ -596,7 +597,7 @@ void MotionPlanningFrame::computeDeleteSceneButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } } planning_display_->addMainLoopJob([this] { populatePlanningSceneTreeView(); }); @@ -622,7 +623,7 @@ void MotionPlanningFrame::computeDeleteQueryButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } planning_display_->addMainLoopJob([this, s] { computeDeleteQueryButtonClickedHelper(s); }); } @@ -684,7 +685,7 @@ void MotionPlanningFrame::computeLoadSceneButtonClicked() if (s->type() == ITEM_TYPE_SCENE) { std::string scene = s->text(0).toStdString(); - RCLCPP_DEBUG(logger_, "Attempting to load scene '%s'", scene.c_str()); + RCLCPP_DEBUG(LOGGER, "Attempting to load scene '%s'", scene.c_str()); moveit_warehouse::PlanningSceneWithMetadata scene_m; bool got_ps = false; @@ -694,17 +695,17 @@ void MotionPlanningFrame::computeLoadSceneButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } if (got_ps) { - RCLCPP_INFO(logger_, "Loaded scene '%s'", scene.c_str()); + RCLCPP_INFO(LOGGER, "Loaded scene '%s'", scene.c_str()); if (planning_display_->getPlanningSceneMonitor()) { if (scene_m->robot_model_name != planning_display_->getRobotModel()->getName()) { - RCLCPP_INFO(logger_, + RCLCPP_INFO(LOGGER, "Scene '%s' was saved for robot '%s' but we are using robot '%s'. Using scene geometry only", scene.c_str(), scene_m->robot_model_name.c_str(), planning_display_->getRobotModel()->getName().c_str()); @@ -723,7 +724,7 @@ void MotionPlanningFrame::computeLoadSceneButtonClicked() } else { - RCLCPP_WARN(logger_, "Failed to load scene '%s'. Has the message format changed since the scene was saved?", + RCLCPP_WARN(LOGGER, "Failed to load scene '%s'. Has the message format changed since the scene was saved?", scene.c_str()); } } @@ -752,7 +753,7 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } if (got_q) @@ -779,7 +780,7 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() } else { - RCLCPP_ERROR(logger_, + RCLCPP_ERROR(LOGGER, "Failed to load planning query '%s'. Has the message format changed since the query was saved?", query_name.c_str()); } @@ -1049,10 +1050,10 @@ void MotionPlanningFrame::computeExportGeometryAsText(const std::string& path) { ps->saveGeometryToStream(fout); fout.close(); - RCLCPP_INFO(logger_, "Saved current scene geometry to '%s'", p.c_str()); + RCLCPP_INFO(LOGGER, "Saved current scene geometry to '%s'", p.c_str()); } else - RCLCPP_WARN(logger_, "Unable to save current scene geometry to '%s'", p.c_str()); + RCLCPP_WARN(LOGGER, "Unable to save current scene geometry to '%s'", p.c_str()); } } @@ -1064,7 +1065,7 @@ void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path) std::ifstream fin(path.c_str()); if (ps->loadGeometryFromStream(fin)) { - RCLCPP_INFO(logger_, "Loaded scene geometry from '%s'", path.c_str()); + RCLCPP_INFO(LOGGER, "Loaded scene geometry from '%s'", path.c_str()); planning_display_->addMainLoopJob([this] { populateCollisionObjectsList(); }); planning_display_->queueRenderSceneGeometry(); setLocalSceneEdited(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index e79a026806..eab7bfc328 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -50,6 +50,7 @@ namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_planning"); void MotionPlanningFrame::planButtonClicked() { @@ -99,7 +100,7 @@ void MotionPlanningFrame::pathConstraintsIndexChanged(int index) { std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString(); if (!move_group_->setPathConstraints(c)) - RCLCPP_WARN_STREAM(logger_, "Unable to set the path constraints: " << c); + RCLCPP_WARN_STREAM(LOGGER, "Unable to set the path constraints: " << c); } else move_group_->clearPathConstraints(); @@ -113,7 +114,7 @@ void MotionPlanningFrame::onClearOctomapClicked() if (result.wait_for(std::chrono::seconds(0)) != std::future_status::ready) { - RCLCPP_ERROR(logger_, "Failed to call clear_octomap_service"); + RCLCPP_ERROR(LOGGER, "Failed to call clear_octomap_service"); } ui_->clear_octomap_button->setEnabled(false); } @@ -128,7 +129,7 @@ bool MotionPlanningFrame::computeCartesianPlan() const moveit::core::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name); if (!link) { - RCLCPP_ERROR_STREAM(logger_, "Failed to determine unique end-effector link: " << link_name); + RCLCPP_ERROR_STREAM(LOGGER, "Failed to determine unique end-effector link: " << link_name); return false; } waypoints.push_back(tf2::toMsg(goal.getGlobalLinkTransform(link))); @@ -145,7 +146,7 @@ bool MotionPlanningFrame::computeCartesianPlan() if (fraction >= 1.0) { - RCLCPP_INFO(logger_, "Achieved %f %% of Cartesian path", fraction * 100.); + RCLCPP_INFO(LOGGER, "Achieved %f %% of Cartesian path", fraction * 100.); // Compute time parameterization to also provide velocities // https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4 @@ -154,7 +155,7 @@ bool MotionPlanningFrame::computeCartesianPlan() trajectory_processing::TimeOptimalTrajectoryGeneration time_parameterization; bool success = time_parameterization.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(), ui_->acceleration_scaling_factor->value()); - RCLCPP_INFO(logger_, "Computing time stamps %s", success ? "SUCCEEDED" : "FAILED"); + RCLCPP_INFO(LOGGER, "Computing time stamps %s", success ? "SUCCEEDED" : "FAILED"); // Store trajectory in current_plan_ current_plan_ = std::make_shared(); @@ -346,11 +347,11 @@ void MotionPlanningFrame::updateQueryStateHelper(moveit::core::RobotState& state } // Explain if no valid rand state found if (attempt_count >= MAX_ATTEMPTS) - RCLCPP_WARN(logger_, "Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS); + RCLCPP_WARN(LOGGER, "Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS); } else { - RCLCPP_WARN_STREAM(logger_, "Unable to get joint model group " << planning_display_->getCurrentPlanningGroup()); + RCLCPP_WARN_STREAM(LOGGER, "Unable to get joint model group " << planning_display_->getCurrentPlanningGroup()); } return; } @@ -418,7 +419,7 @@ void MotionPlanningFrame::populatePlannersList(const std::vectorgetCurrentPlanningGroup(); - RCLCPP_DEBUG(logger_, "Found %zu planners for group '%s' and pipeline '%s'", desc.planner_ids.size(), group.c_str(), + RCLCPP_DEBUG(LOGGER, "Found %zu planners for group '%s' and pipeline '%s'", desc.planner_ids.size(), group.c_str(), desc.pipeline_id.c_str()); ui_->planning_algorithm_combo_box->clear(); @@ -432,7 +433,7 @@ void MotionPlanningFrame::populatePlannerDescription(const moveit_msgs::msg::Pla { for (const std::string& planner_id : desc.planner_ids) { - RCLCPP_DEBUG(logger_, "planner id: %s", planner_id.c_str()); + RCLCPP_DEBUG(LOGGER, "planner id: %s", planner_id.c_str()); if (planner_id == group) { found_group = true; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp index 245b750269..faf3a538c5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp @@ -57,6 +57,7 @@ namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_scenes"); void MotionPlanningFrame::saveSceneButtonClicked() { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 35d84a425e..bf282d162d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -46,6 +46,7 @@ namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_states"); void MotionPlanningFrame::populateRobotStatesList() { @@ -102,7 +103,7 @@ void MotionPlanningFrame::loadStoredStates(const std::string& pattern) } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } if (!got_state) continue; @@ -157,7 +158,7 @@ void MotionPlanningFrame::saveRobotStateButtonClicked(const moveit::core::RobotS } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Cannot save robot state on the database: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Cannot save robot state on the database: %s", ex.what()); } } else @@ -233,7 +234,7 @@ void MotionPlanningFrame::removeStateButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "%s", ex.what()); + RCLCPP_ERROR(LOGGER, "%s", ex.what()); } } break; diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 5fa7fe9019..3e37370448 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -221,7 +221,6 @@ protected Q_SLOTS: // rclcpp node rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; }; } // namespace moveit_rviz_plugin 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..23b16c06ab 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 @@ -37,12 +37,14 @@ #include #include #include -#include namespace moveit { namespace tools { +// Logger +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_background_processing.background_processing"); + BackgroundProcessing::BackgroundProcessing() { // spin a thread that will process user events @@ -79,14 +81,13 @@ void BackgroundProcessing::processingThread() action_lock_.unlock(); try { - RCLCPP_DEBUG(moveit::getLogger(), "Begin executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Begin executing '%s'", action_name.c_str()); fn(); - RCLCPP_DEBUG(moveit::getLogger(), "Done executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(LOGGER, "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(LOGGER, "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..75de575ec9 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 @@ -56,18 +56,15 @@ #include #include -#include namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.planning_scene_display"); // ****************************************************************************************** // Base class constructor // ****************************************************************************************** PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool show_scene_robot) - : Display() - , planning_scene_needs_render_(true) - , current_scene_time_(0.0f) - , logger_(moveit::makeChildLogger("planning_scene_display")) + : Display(), planning_scene_needs_render_(true), current_scene_time_(0.0f) { move_group_ns_property_ = new rviz_common::properties::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in " @@ -276,7 +273,7 @@ void PlanningSceneDisplay::executeMainLoopJobs() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Exception caught executing main loop job: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught executing main loop job: %s", ex.what()); } main_loop_jobs_lock_.lock(); } @@ -378,7 +375,7 @@ void PlanningSceneDisplay::renderPlanningScene() } catch (std::exception& ex) { - RCLCPP_ERROR(logger_, "Caught %s while rendering planning scene", ex.what()); + RCLCPP_ERROR(LOGGER, "Caught %s while rendering planning scene", ex.what()); } planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool()); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 2e11e94c61..48a16ad598 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -42,7 +42,6 @@ #include #include #include -#include #ifndef Q_MOC_RUN #include @@ -165,7 +164,6 @@ private Q_SLOTS: rclcpp::Node::SharedPtr node_; TrajectoryPanel* trajectory_slider_panel_; rviz_common::PanelDockWidget* trajectory_slider_dock_panel_; - rclcpp::Logger logger_; // Properties rviz_common::properties::BoolProperty* display_path_visual_enabled_property_; 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..1b82869435 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 @@ -42,7 +42,7 @@ #include #include #include -#include + #include #include #include @@ -62,6 +62,7 @@ using namespace std::placeholders; namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_rviz_plugin_render_tools.trajectory_visualization"); TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Property* widget, rviz_common::Display* display) @@ -72,7 +73,6 @@ TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Proper , widget_(widget) , trajectory_slider_panel_(nullptr) , trajectory_slider_dock_panel_(nullptr) - , logger_(moveit::makeChildLogger("trajectory_visualization")) { trajectory_topic_property_ = new rviz_common::properties::RosTopicProperty( "Trajectory Topic", "/display_planned_path", @@ -201,7 +201,7 @@ void TrajectoryVisualization::onRobotModelLoaded(const moveit::core::RobotModelC // Error check if (!robot_model_) { - RCLCPP_ERROR_STREAM(logger_, "No robot model found"); + RCLCPP_ERROR_STREAM(LOGGER, "No robot model found"); return; } @@ -550,13 +550,13 @@ void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::msg:: // Error check if (!robot_state_ || !robot_model_) { - RCLCPP_ERROR_STREAM(logger_, "No robot state or robot model loaded"); + RCLCPP_ERROR_STREAM(LOGGER, "No robot state or robot model loaded"); return; } if (!msg->model_id.empty() && msg->model_id != robot_model_->getName()) { - RCLCPP_WARN(logger_, "Received a trajectory to display for model '%s' but model '%s' was expected", + RCLCPP_WARN(LOGGER, "Received a trajectory to display for model '%s' but model '%s' was expected", msg->model_id.c_str(), robot_model_->getName().c_str()); } diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h index 4d15023463..46f4082810 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h @@ -95,7 +95,6 @@ private Q_SLOTS: rviz_common::properties::StringProperty* robot_description_property_; rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; }; } // namespace moveit_rviz_plugin 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..701cd3e294 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -38,15 +38,14 @@ #include #include -#include #include namespace moveit_rviz_plugin { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.visualization.trajectory_display"); -TrajectoryDisplay::TrajectoryDisplay() : Display(), logger_(moveit::makeChildLogger("trajectory_display")) - +TrajectoryDisplay::TrajectoryDisplay() : Display() { // The robot description property is only needed when using the trajectory playback standalone (not within motion // planning plugin) @@ -66,7 +65,7 @@ void TrajectoryDisplay::onInitialize() auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); if (!ros_node_abstraction) { - RCLCPP_INFO(logger_, "Unable to lock weak_ptr from DisplayContext in TrajectoryDisplay constructor"); + RCLCPP_INFO(LOGGER, "Unable to lock weak_ptr from DisplayContext in TrajectoryDisplay constructor"); return; } node_ = ros_node_abstraction->get_raw_node(); diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index d2b35bf596..5124fa000e 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/src/broadcast.cpp @@ -75,7 +75,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::getLoggerMut() = node->get_logger(); // time to wait in between publishing messages double delay = 0.001; diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index d8efdf869b..c345d033aa 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -223,7 +223,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::getLoggerMut() = node->get_logger(); // clang-format off boost::program_options::options_description desc; diff --git a/moveit_ros/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/src/initialize_demo_db.cpp index 23d92f6bc7..4e93225909 100644 --- a/moveit_ros/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/src/initialize_demo_db.cpp @@ -65,7 +65,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::getLoggerMut() = node->get_logger(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index 7a1893c983..96e9780db3 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -89,7 +89,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::getLoggerMut() = node->get_logger(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index 2dbcc20fc0..f4d68a10ad 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -138,7 +138,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::getLoggerMut() = node->get_logger(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), diff --git a/moveit_ros/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/src/warehouse_services.cpp index 747ee1d2ee..7234adc9d3 100644 --- a/moveit_ros/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/src/warehouse_services.cpp @@ -141,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("moveit_warehouse_services", node_options); - moveit::setLogger(node->get_logger()); + moveit::getLoggerMut() = node->get_logger(); std::string host;