From b9efa98e6f7eaa37a2b7de24b7b728d7ea9e7d91 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 24 Oct 2023 16:38:53 -0600 Subject: [PATCH] Use node logging in moveit_ros Signed-off-by: Tyler Weaver --- .../utils/include/moveit/utils/logger.hpp | 6 +- moveit_core/utils/src/logger.cpp | 16 +- moveit_core/utils/test/CMakeLists.txt | 15 +- 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 | 98 ++++++------ .../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 | 16 +- .../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 | 13 +- .../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 | 121 +++++++-------- .../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 +- 122 files changed, 1160 insertions(+), 973 deletions(-) create mode 100644 moveit_core/utils/test/logger_from_only_child_dut.cpp diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 7d3039f02ba..0e10a37da5a 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -43,7 +43,7 @@ namespace moveit { // Function for getting a reference to a logger object kept in a global variable. -// Use get_logger_mut to set the logger to a node logger. +// Use set_logger to set the logger to a node logger. const rclcpp::Logger& get_logger(); // Function for getting a child logger. In Humble this also creates a node. @@ -51,7 +51,7 @@ const rclcpp::Logger& get_logger(); // instead store it in the state of your class or method. rclcpp::Logger make_child_logger(const std::string& name); -// Mutable access to global logger for setting to node logger. -rclcpp::Logger& get_logger_mut(); +// Set the global logger +void set_logger(const rclcpp::Logger& logger); } // namespace moveit diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index a6a314f1601..2d21d431c39 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -43,9 +43,16 @@ namespace moveit { +rclcpp::Logger& get_global_logger_ref() +{ + static std::shared_ptr moveit_node = std::make_shared("moveit"); + static rclcpp::Logger logger = moveit_node->get_logger(); + return logger; +} + const rclcpp::Logger& get_logger() { - return get_logger_mut(); + return get_global_logger_ref(); } rclcpp::Logger make_child_logger(const std::string& name) @@ -65,13 +72,12 @@ rclcpp::Logger make_child_logger(const std::string& name) } #endif - return get_logger_mut().get_child(name); + return get_global_logger_ref().get_child(name); } -rclcpp::Logger& get_logger_mut() +void set_logger(const rclcpp::Logger& logger) { - static rclcpp::Logger logger = rclcpp::get_logger("moveit"); - return logger; + get_global_logger_ref() = logger; } } // namespace moveit diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt index bb52a7e292b..cc1a768cef6 100644 --- a/moveit_core/utils/test/CMakeLists.txt +++ b/moveit_core/utils/test/CMakeLists.txt @@ -5,21 +5,34 @@ 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" ) + +# 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" +) diff --git a/moveit_core/utils/test/logger_dut.cpp b/moveit_core/utils/test/logger_dut.cpp index ca1525f1fcd..5da21b569fa 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(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 a444d038b7a..6523669cdbd 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(node->get_logger()); // Make a child logger const auto child_logger = moveit::make_child_logger("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 new file mode 100644 index 00000000000..78cb4cea1a7 --- /dev/null +++ b/moveit_core/utils/test/logger_from_only_child_dut.cpp @@ -0,0 +1,53 @@ +/********************************************************************* + * 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::make_child_logger("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 e55267689cd..88b321f39c1 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -41,6 +41,7 @@ #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. @@ -61,10 +62,9 @@ #undef max +using moveit::get_logger; 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(LOGGER, "Cannot find pipeline '%s'", planning_pipeline_name.c_str()); + RCLCPP_ERROR(get_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(LOGGER, "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); + RCLCPP_ERROR(get_logger(), "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); continue; } } @@ -124,14 +124,15 @@ BenchmarkExecutor::~BenchmarkExecutor() // Error check if (moveit_cpp_->getPlanningPipelines().empty()) { - RCLCPP_ERROR(LOGGER, "No planning pipelines have been loaded. Nothing to do for the benchmarking service."); + RCLCPP_ERROR(get_logger(), "No planning pipelines have been loaded. Nothing to do for the benchmarking service."); } else { - RCLCPP_INFO(LOGGER, "Available planning pipelines:"); + RCLCPP_INFO(get_logger(), "Available planning pipelines:"); for (const std::pair& entry : moveit_cpp_->getPlanningPipelines()) - RCLCPP_INFO_STREAM(LOGGER, "Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); + RCLCPP_INFO_STREAM(get_logger(), + "Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); } return true; } @@ -202,7 +203,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& options) { if (moveit_cpp_->getPlanningPipelines().empty()) { - RCLCPP_ERROR(LOGGER, "No planning pipelines configured. Did you call BenchmarkExecutor::initialize?"); + RCLCPP_ERROR(get_logger(), "No planning pipelines configured. Did you call BenchmarkExecutor::initialize?"); return false; } @@ -234,7 +235,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& options) query_start_fn(queries[i].request, planning_scene_); } - RCLCPP_INFO(LOGGER, "Benchmarking query '%s' (%lu of %lu)", queries[i].name.c_str(), i + 1, queries.size()); + RCLCPP_INFO(get_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; @@ -271,12 +272,12 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& options, if (!loadBenchmarkQueryData(options, scene_msg, start_states, path_constraints, goal_constraints, traj_constraints, queries)) { - RCLCPP_ERROR(LOGGER, "Failed to load benchmark query data"); + RCLCPP_ERROR(get_logger(), "Failed to load benchmark query data"); return false; } RCLCPP_INFO( - LOGGER, + get_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()); @@ -402,45 +403,45 @@ bool BenchmarkExecutor::loadBenchmarkQueryData( constraints_storage_ = std::make_shared(warehouse_connection); trajectory_constraints_storage_ = std::make_shared(warehouse_connection); - RCLCPP_INFO(LOGGER, "Connected to DB"); + RCLCPP_INFO(get_logger(), "Connected to DB"); } else { - RCLCPP_ERROR(LOGGER, "Failed to connect to DB"); + RCLCPP_ERROR(get_logger(), "Failed to connect to DB"); return false; } } catch (std::exception& e) { - RCLCPP_ERROR(LOGGER, "Failed to initialize benchmark server: '%s'", e.what()); + RCLCPP_ERROR(get_logger(), "Failed to initialize benchmark server: '%s'", e.what()); return false; } if (!loadPlanningScene(options.scene_name, scene_msg)) { - RCLCPP_ERROR(LOGGER, "Failed to load the planning scene"); + RCLCPP_ERROR(get_logger(), "Failed to load the planning scene"); return false; } if (!loadStates(options.start_state_regex, start_states)) { - RCLCPP_ERROR(LOGGER, "Failed to load the states"); + RCLCPP_ERROR(get_logger(), "Failed to load the states"); return false; } if (!loadPathConstraints(options.goal_constraint_regex, goal_constraints)) { - RCLCPP_ERROR(LOGGER, "Failed to load the goal constraints"); + RCLCPP_ERROR(get_logger(), "Failed to load the goal constraints"); } if (!loadPathConstraints(options.path_constraint_regex, path_constraints)) { - RCLCPP_ERROR(LOGGER, "Failed to load the path constraints"); + RCLCPP_ERROR(get_logger(), "Failed to load the path constraints"); } if (!loadTrajectoryConstraints(options.trajectory_constraint_regex, traj_constraints)) { - RCLCPP_ERROR(LOGGER, "Failed to load the trajectory constraints"); + RCLCPP_ERROR(get_logger(), "Failed to load the trajectory constraints"); } if (!loadQueries(options.query_regex, options.scene_name, queries)) { - RCLCPP_ERROR(LOGGER, "Failed to get a query regex"); + RCLCPP_ERROR(get_logger(), "Failed to get a query regex"); } return true; } @@ -535,7 +536,7 @@ bool BenchmarkExecutor::plannerConfigurationsExist( if (!pipeline_exists) { - RCLCPP_ERROR(LOGGER, "Planning pipeline '%s' does NOT exist", pipeline_config_entry.first.c_str()); + RCLCPP_ERROR(get_logger(), "Planning pipeline '%s' does NOT exist", pipeline_config_entry.first.c_str()); return false; } } @@ -565,8 +566,8 @@ bool BenchmarkExecutor::plannerConfigurationsExist( if (!planner_exists) { - 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()); + RCLCPP_ERROR(get_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'; @@ -588,7 +589,7 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_ if (!planning_scene_storage_->getPlanningScene(planning_scene_w_metadata, scene_name)) { - RCLCPP_ERROR(LOGGER, "Failed to load planning scene '%s'", scene_name.c_str()); + RCLCPP_ERROR(get_logger(), "Failed to load planning scene '%s'", scene_name.c_str()); return false; } scene_msg = static_cast(*planning_scene_w_metadata); @@ -598,7 +599,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(LOGGER, "Failed to load planning scene world '%s'", scene_name.c_str()); + RCLCPP_ERROR(get_logger(), "Failed to load planning scene world '%s'", scene_name.c_str()); return false; } scene_msg.world = static_cast(*pswwm); @@ -607,16 +608,16 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_ } else { - RCLCPP_ERROR(LOGGER, "Failed to find planning scene '%s'", scene_name.c_str()); + RCLCPP_ERROR(get_logger(), "Failed to find planning scene '%s'", scene_name.c_str()); return false; } } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Error loading planning scene: %s", ex.what()); + RCLCPP_ERROR(get_logger(), "Error loading planning scene: %s", ex.what()); return false; } - RCLCPP_INFO(LOGGER, "Loaded planning scene successfully"); + RCLCPP_INFO(get_logger(), "Loaded planning scene successfully"); return true; } @@ -625,7 +626,7 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& { if (regex.empty()) { - RCLCPP_WARN(LOGGER, "No query regex provided, don't load any queries from the database"); + RCLCPP_WARN(get_logger(), "No query regex provided, don't load any queries from the database"); return true; } @@ -636,13 +637,13 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Error loading motion planning queries: %s", ex.what()); + RCLCPP_ERROR(get_logger(), "Error loading motion planning queries: %s", ex.what()); return false; } if (query_names.empty()) { - RCLCPP_ERROR(LOGGER, "Scene '%s' has no associated queries", scene_name.c_str()); + RCLCPP_ERROR(get_logger(), "Scene '%s' has no associated queries", scene_name.c_str()); return false; } @@ -655,7 +656,7 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what()); + RCLCPP_ERROR(get_logger(), "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what()); continue; } @@ -664,7 +665,7 @@ bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& query.request = static_cast(*planning_query); queries.push_back(query); } - RCLCPP_INFO(LOGGER, "Loaded queries successfully"); + RCLCPP_INFO(get_logger(), "Loaded queries successfully"); return true; } @@ -678,7 +679,7 @@ bool BenchmarkExecutor::loadStates(const std::string& regex, std::vector& responses, const std::vector& solved) { - RCLCPP_INFO(LOGGER, "Computing result path similarity"); + RCLCPP_INFO(get_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()); @@ -1236,7 +1238,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& benchmark_request, c std::ofstream out(filename.c_str()); if (!out) { - RCLCPP_ERROR(LOGGER, "Failed to open '%s' for benchmark output", filename.c_str()); + RCLCPP_ERROR(get_logger(), "Failed to open '%s' for benchmark output", filename.c_str()); return; } @@ -1374,5 +1376,5 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& benchmark_request, c } out.close(); - RCLCPP_INFO(LOGGER, "Benchmark results saved to '%s'", filename.c_str()); + RCLCPP_INFO(get_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 e3483245400..e9f56052698 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::get_logger; 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(LOGGER, "Benchmark scene_name NOT specified"); + RCLCPP_WARN(get_logger(), "Benchmark scene_name NOT specified"); } - 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()); + RCLCPP_INFO(get_logger(), "Benchmark host: %s", hostname.c_str()); + RCLCPP_INFO(get_logger(), "Benchmark port: %d", port); + RCLCPP_INFO(get_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(LOGGER, "Benchmark group NOT specified"); + RCLCPP_WARN(get_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(LOGGER, "Workspace frame_id not specified in benchmark config"); + RCLCPP_WARN(get_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(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), + RCLCPP_INFO(get_logger(), "Benchmark name: '%s'", benchmark_name.c_str()); + RCLCPP_INFO(get_logger(), "Benchmark #runs: %d", runs); + RCLCPP_INFO(get_logger(), "Benchmark timeout: %f secs", timeout); + RCLCPP_INFO(get_logger(), "Benchmark group: %s", group_name.c_str()); + RCLCPP_INFO(get_logger(), "Benchmark query regex: '%s'", query_regex.c_str()); + RCLCPP_INFO(get_logger(), "Benchmark start state regex: '%s':", start_state_regex.c_str()); + RCLCPP_INFO(get_logger(), "Benchmark goal constraint regex: '%s':", goal_constraint_regex.c_str()); + RCLCPP_INFO(get_logger(), "Benchmark path constraint regex: '%s':", path_constraint_regex.c_str()); + RCLCPP_INFO(get_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(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 << ']'); + RCLCPP_INFO(get_logger(), "Benchmark output directory: %s", output_directory.c_str()); + RCLCPP_INFO_STREAM(get_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(LOGGER, "No benchmark_config found on param server"); + RCLCPP_ERROR(get_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(LOGGER, "No single planning pipeline namespace `%s` configured.", (ns + ".pipelines").c_str()); + RCLCPP_WARN(get_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(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_parameter_name.c_str()); + RCLCPP_ERROR(get_logger(), "Fail to get the parameter in `%s` namespace.", pipeline_parameter_name.c_str()); return false; } - RCLCPP_INFO(LOGGER, "Reading in planner names for planning pipeline '%s'", pipeline_name.c_str()); + RCLCPP_INFO(get_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(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_parameter_planners.c_str()); + RCLCPP_ERROR(get_logger(), "Fail to get the parameter in `%s` namespace.", pipeline_parameter_planners.c_str()); return false; } for (const std::string& planner : planners) { - RCLCPP_INFO(LOGGER, " %s", planner.c_str()); + RCLCPP_INFO(get_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(LOGGER, "No parallel planning pipeline namespace `%s` configured.", + RCLCPP_WARN(get_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(LOGGER, "Reading in parameters for parallel planning pipeline '%s'", parallel_pipeline.c_str()); + RCLCPP_INFO(get_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(LOGGER, "Fail to get the parameter in `%s` namespace.", pipelines_parameter.c_str()); + RCLCPP_ERROR(get_logger(), "Fail to get the parameter in `%s` namespace.", pipelines_parameter.c_str()); return false; } @@ -235,13 +235,14 @@ 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(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_planner_ids_parameter.c_str()); + RCLCPP_ERROR(get_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(LOGGER, "Number of planner ids is unequal to the number of pipelines in %s.", + RCLCPP_ERROR(get_logger(), "Number of planner ids is unequal to the number of pipelines in %s.", parallel_pipeline.c_str()); return false; } @@ -256,17 +257,17 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) for (const auto& entry : parallel_planning_pipelines) { - RCLCPP_INFO(LOGGER, "Parallel planning pipeline '%s'", entry.first.c_str()); + RCLCPP_INFO(get_logger(), "Parallel planning pipeline '%s'", entry.first.c_str()); for (const auto& pair : entry.second) { - RCLCPP_INFO(LOGGER, " '%s': '%s'", pair.first.c_str(), pair.second.c_str()); + RCLCPP_INFO(get_logger(), " '%s': '%s'", pair.first.c_str(), pair.second.c_str()); } } } } if (pipelines.empty() && parallel_pipelines.empty()) { - RCLCPP_ERROR(LOGGER, "No single or parallel planning pipelines configured for benchmarking."); + RCLCPP_ERROR(get_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 d12952b1688..cee83a5ad99 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -44,8 +44,9 @@ #include #include #include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.benchmarks.RunBenchmark"); +using moveit::get_logger; int main(int argc, char** argv) { @@ -54,6 +55,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("moveit_run_benchmark", node_options); + moveit::set_logger(node->get_logger()); // Read benchmark options from param server moveit_ros_benchmarks::BenchmarkOptions options(node); @@ -64,7 +66,7 @@ int main(int argc, char** argv) options.getPlanningPipelineNames(planning_pipelines); if (!server.initialize(planning_pipelines)) { - RCLCPP_ERROR(LOGGER, "Failed to initialize benchmark server."); + RCLCPP_ERROR(get_logger(), "Failed to initialize benchmark server."); rclcpp::shutdown(); return 1; } @@ -81,18 +83,18 @@ int main(int argc, char** argv) { auto planning_scene_storage = moveit_warehouse::PlanningSceneStorage(warehouse_connection); planning_scene_storage.getPlanningSceneNames(scene_names); - RCLCPP_INFO(LOGGER, "Loaded scene names"); + RCLCPP_INFO(get_logger(), "Loaded scene names"); } else { - RCLCPP_ERROR(LOGGER, "Failed to load scene names from DB"); + RCLCPP_ERROR(get_logger(), "Failed to load scene names from DB"); rclcpp::shutdown(); return 1; } } catch (std::exception& e) { - RCLCPP_ERROR(LOGGER, "Failed to load scene names from DB: '%s'", e.what()); + RCLCPP_ERROR(get_logger(), "Failed to load scene names from DB: '%s'", e.what()); rclcpp::shutdown(); return 1; } @@ -102,7 +104,7 @@ int main(int argc, char** argv) options.scene_name = name; if (!server.runBenchmarks(options)) { - RCLCPP_ERROR(LOGGER, "Failed to run all benchmarks"); + RCLCPP_ERROR(get_logger(), "Failed to run all benchmarks"); } } } @@ -110,11 +112,11 @@ int main(int argc, char** argv) { if (!server.runBenchmarks(options)) { - RCLCPP_ERROR(LOGGER, "Failed to run all benchmarks"); + RCLCPP_ERROR(get_logger(), "Failed to run all benchmarks"); } } - RCLCPP_INFO(LOGGER, "Finished benchmarking"); + RCLCPP_INFO(get_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 a70ebe46850..4b1b04cd561 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,7 +42,6 @@ namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("global_planner_component"); const auto JOIN_THREAD_TIMEOUT = std::chrono::seconds(1); } // namespace @@ -68,7 +67,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() node_->get_parameter("global_planning_action_name", global_planning_action_name); if (global_planning_action_name.empty()) { - RCLCPP_ERROR(LOGGER, "global_planning_action_name was not defined"); + RCLCPP_ERROR(node_->get_logger(), "global_planning_action_name was not defined"); return false; } cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -77,7 +76,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() // Goal callback [this](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(LOGGER, "Received global planning goal request"); + RCLCPP_INFO(node_->get_logger(), "Received global planning goal request"); // If another goal is active, cancel it and reject this goal if (long_callback_thread_.joinable()) { @@ -85,7 +84,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(LOGGER, "Another goal was running. Rejecting the new hybrid planning goal."); + RCLCPP_WARN(node_->get_logger(), "Another goal was running. Rejecting the new hybrid planning goal."); return rclcpp_action::GoalResponse::REJECT; } if (!global_planner_instance_->reset()) @@ -97,7 +96,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() }, // Cancel callback [this](const std::shared_ptr>& /*unused*/) { - RCLCPP_INFO(LOGGER, "Received request to cancel global planning goal"); + RCLCPP_INFO(node_->get_logger(), "Received request to cancel global planning goal"); if (long_callback_thread_.joinable()) { long_callback_thread_.join(); @@ -132,7 +131,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while creating global planner plugin loader: '%s'", ex.what()); + RCLCPP_ERROR(node_->get_logger(), "Exception while creating global planner plugin loader: '%s'", ex.what()); return false; } try @@ -141,17 +140,18 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while loading global planner '%s': '%s'", planner_plugin_name_.c_str(), ex.what()); + RCLCPP_ERROR(node_->get_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(LOGGER, "Unable to initialize global planner plugin '%s'", planner_plugin_name_.c_str()); + RCLCPP_ERROR(node_->get_logger(), "Unable to initialize global planner plugin '%s'", planner_plugin_name_.c_str()); return false; } - RCLCPP_INFO(LOGGER, "Using global planner plugin '%s'", planner_plugin_name_.c_str()); + RCLCPP_INFO(node_->get_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 47d0d97e4a7..5a487e8708a 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,11 +36,6 @@ #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."; @@ -101,7 +96,8 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( if ((global_goal_handle->get_goal())->motion_sequence.items.empty()) { - RCLCPP_WARN(LOGGER, "Global planner received motion sequence request with no items. At least one is needed."); + RCLCPP_WARN(node_ptr_->get_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; } @@ -109,9 +105,10 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( // Process goal if ((global_goal_handle->get_goal())->motion_sequence.items.size() > 1) { - 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!"); + 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!"); } 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 b1584123ad8..5180fe7542c 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,11 +35,6 @@ #include #include -namespace -{ -const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); -} // namespace - namespace moveit::hybrid_planning { using namespace std::chrono_literals; @@ -77,7 +72,7 @@ bool HybridPlanningManager::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while creating planner logic plugin loader '%s'", ex.what()); + RCLCPP_ERROR(get_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 = ""; @@ -97,11 +92,11 @@ bool HybridPlanningManager::initialize() { throw std::runtime_error("Unable to initialize planner logic plugin"); } - RCLCPP_INFO(LOGGER, "Using planner logic interface '%s'", logic_plugin_name.c_str()); + RCLCPP_INFO(get_logger(), "Using planner logic interface '%s'", logic_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what()); + RCLCPP_ERROR(get_logger(), "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what()); } // Initialize local planning action client @@ -109,14 +104,14 @@ bool HybridPlanningManager::initialize() get_parameter("local_planning_action_name", local_planning_action_name); if (local_planning_action_name.empty()) { - RCLCPP_ERROR(LOGGER, "local_planning_action_name parameter was not defined"); + RCLCPP_ERROR(get_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(LOGGER, "Local planner action server not available after waiting"); + RCLCPP_ERROR(get_logger(), "Local planner action server not available after waiting"); return false; } @@ -125,14 +120,14 @@ bool HybridPlanningManager::initialize() get_parameter("global_planning_action_name", global_planning_action_name); if (global_planning_action_name.empty()) { - RCLCPP_ERROR(LOGGER, "global_planning_action_name parameter was not defined"); + RCLCPP_ERROR(get_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(LOGGER, "Global planner action server not available after waiting"); + RCLCPP_ERROR(get_logger(), "Global planner action server not available after waiting"); return false; } @@ -141,7 +136,7 @@ bool HybridPlanningManager::initialize() get_parameter("hybrid_planning_action_name", hybrid_planning_action_name); if (hybrid_planning_action_name.empty()) { - RCLCPP_ERROR(LOGGER, "hybrid_planning_action_name parameter was not defined"); + RCLCPP_ERROR(get_logger(), "hybrid_planning_action_name parameter was not defined"); return false; } cb_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -149,15 +144,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 - [](const rclcpp_action::GoalUUID& /*unused*/, - const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(LOGGER, "Received goal request"); + [this](const rclcpp_action::GoalUUID& /*unused*/, + const std::shared_ptr& /*unused*/) { + RCLCPP_INFO(get_logger(), "Received goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, // Cancel callback [this](const std::shared_ptr>& /*unused*/) { cancelHybridManagerGoals(); - RCLCPP_INFO(LOGGER, "Received request to cancel goal"); + RCLCPP_INFO(get_logger(), "Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; }, // Execution callback @@ -183,7 +178,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(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + RCLCPP_ERROR(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); } }); return true; @@ -221,7 +216,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(LOGGER, "Hybrid Planning Manager failed to react to " << reaction_result.event); + RCLCPP_ERROR_STREAM(get_logger(), "Hybrid Planning Manager failed to react to " << reaction_result.event); } } @@ -272,7 +267,7 @@ bool HybridPlanningManager::sendGlobalPlannerAction() result->error_message = reaction_result.error_message; hybrid_planning_goal_handle_->abort(result); - RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + RCLCPP_ERROR(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); } }; @@ -327,7 +322,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(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + RCLCPP_ERROR(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); } }; @@ -361,7 +356,7 @@ bool HybridPlanningManager::sendLocalPlannerAction() return result; }()); hybrid_planning_goal_handle_->abort(std::make_shared(result)); - RCLCPP_ERROR_STREAM(LOGGER, "Hybrid Planning Manager failed to react to " << reaction_result.event); + RCLCPP_ERROR_STREAM(get_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 fe774dc01fa..13086091bc2 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,7 +39,6 @@ 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 @@ -85,7 +84,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(LOGGER, *node_->get_clock(), 2000 /* ms */, "The local planner is solving..."); + RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 2000 /* ms */, "The local planner is solving..."); #pragma GCC diagnostic pop // Create controller command trajectory @@ -131,7 +130,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(LOGGER, "Collision ahead, holding current position"); + RCLCPP_INFO(node_->get_logger(), "Collision ahead, holding current position"); // Keep current position moveit::core::RobotState current_state_command(*current_state); if (current_state_command.hasVelocities()) @@ -163,7 +162,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(LOGGER, "The local planner has been stuck for several iterations. Aborting."); + RCLCPP_INFO(node_->get_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 bd1173b5a1f..fb91d7856f2 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,7 +47,6 @@ 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 @@ -77,8 +76,9 @@ bool LocalPlannerComponent::initialize() if ((config_.publish_joint_positions && config_.publish_joint_velocities) || (!config_.publish_joint_positions && !config_.publish_joint_velocities)) { - RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, you must select positions OR velocities. " - "Enabling both or none is not possible!"); + RCLCPP_ERROR(node_->get_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(LOGGER, "Unable to configure planning scene monitor"); + RCLCPP_ERROR(node_->get_logger(), "Unable to configure planning scene monitor"); return false; } @@ -107,7 +107,7 @@ bool LocalPlannerComponent::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while creating trajectory operator plugin loader: '%s'", ex.what()); + RCLCPP_ERROR(node_->get_logger(), "Exception while creating trajectory operator plugin loader: '%s'", ex.what()); return false; } try @@ -117,11 +117,12 @@ 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(LOGGER, "Using trajectory operator interface '%s'", config_.trajectory_operator_plugin_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "Using trajectory operator interface '%s'", + config_.trajectory_operator_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while loading trajectory operator '%s': '%s'", + RCLCPP_ERROR(node_->get_logger(), "Exception while loading trajectory operator '%s': '%s'", config_.trajectory_operator_plugin_name.c_str(), ex.what()); return false; } @@ -134,7 +135,7 @@ bool LocalPlannerComponent::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while creating constraint solver plugin loader '%s'", ex.what()); + RCLCPP_ERROR(node_->get_logger(), "Exception while creating constraint solver plugin loader '%s'", ex.what()); return false; } try @@ -143,11 +144,12 @@ 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(LOGGER, "Using constraint solver interface '%s'", config_.local_constraint_solver_plugin_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "Using constraint solver interface '%s'", + config_.local_constraint_solver_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while loading constraint solver '%s': '%s'", + RCLCPP_ERROR(node_->get_logger(), "Exception while loading constraint solver '%s': '%s'", config_.local_constraint_solver_plugin_name.c_str(), ex.what()); return false; } @@ -159,7 +161,7 @@ bool LocalPlannerComponent::initialize() // Goal callback [this](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(LOGGER, "Received local planning goal request"); + RCLCPP_INFO(node_->get_logger(), "Received local planning goal request"); // If another goal is active, cancel it and reject this goal if (long_callback_thread_.joinable()) { @@ -167,7 +169,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(LOGGER, "Another goal was running. Rejecting the new hybrid planning goal."); + RCLCPP_WARN(node_->get_logger(), "Another goal was running. Rejecting the new hybrid planning goal."); return rclcpp_action::GoalResponse::REJECT; } } @@ -175,7 +177,7 @@ bool LocalPlannerComponent::initialize() }, // Cancel callback [this](const std::shared_ptr>& /*unused*/) { - RCLCPP_INFO(LOGGER, "Received request to cancel local planning goal"); + RCLCPP_INFO(node_->get_logger(), "Received request to cancel local planning goal"); state_ = LocalPlannerState::ABORT; if (long_callback_thread_.joinable()) { @@ -224,7 +226,7 @@ bool LocalPlannerComponent::initialize() }); // Initialize local solution publisher - RCLCPP_INFO(LOGGER, "Using '%s' as local solution topic type", config_.local_solution_topic_type.c_str()); + RCLCPP_INFO(node_->get_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_ = @@ -292,7 +294,7 @@ void LocalPlannerComponent::executeIteration() if (!local_planner_feedback_->feedback.empty()) { local_planning_goal_handle_->publish_feedback(local_planner_feedback_); - RCLCPP_ERROR(LOGGER, "Local planner somehow failed"); + RCLCPP_ERROR(node_->get_logger(), "Local planner somehow failed"); reset(); return; } @@ -348,7 +350,8 @@ 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(LOGGER, "Local planner somehow failed"); // TODO(sjahr) Add more detailed failure information + RCLCPP_ERROR(node_->get_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 b6cbacae06b..ae3038ace14 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,7 +40,6 @@ 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 edbd907a09e..5142f40c2a3 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,11 +37,10 @@ #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") { @@ -68,7 +67,7 @@ bool ApplyPlanningSceneService::applyScene(const std::shared_ptrplanning_scene_monitor_) { - RCLCPP_ERROR(LOGGER, "Cannot apply PlanningScene as no scene is monitored."); + RCLCPP_ERROR(moveit::get_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 60023147690..e2ddfaced48 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,6 +46,9 @@ #include #include #include +#include + +using moveit::get_logger; namespace { @@ -62,9 +65,6 @@ 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(LOGGER, "Received request to compute Cartesian path"); + RCLCPP_INFO(get_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(LOGGER, "Error encountered transforming waypoints to frame '%s'", default_frame.c_str()); + RCLCPP_ERROR(get_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(LOGGER, "Maximum step to take between consecutive configurations along Cartesian path" - "was not specified (this value needs to be > 0)"); + RCLCPP_ERROR(get_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(LOGGER, + RCLCPP_INFO(get_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, @@ -184,7 +184,7 @@ bool MoveGroupCartesianPathService::computeService( time_param.computeTimeStamps(rt, req->max_velocity_scaling_factor, req->max_acceleration_scaling_factor); rt.getRobotTrajectoryMsg(res->solution); - RCLCPP_INFO(LOGGER, "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)", + RCLCPP_INFO(get_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 7c18ed2a3cb..47f1bbad374 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,9 +37,7 @@ #include "clear_octomap_service_capability.h" #include #include - -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_move_group_default_capabilities.clear_octomap_service_capability"); +#include move_group::ClearOctomapService::ClearOctomapService() : MoveGroupCapability("ClearOctomapService") { @@ -57,11 +55,11 @@ void move_group::ClearOctomapService::clearOctomap(const std::shared_ptr& /*res*/) { if (!context_->planning_scene_monitor_) - RCLCPP_ERROR(LOGGER, "Cannot clear octomap since planning_scene_monitor_ does not exist."); + RCLCPP_ERROR(moveit::get_logger(), "Cannot clear octomap since planning_scene_monitor_ does not exist."); - RCLCPP_INFO(LOGGER, "Clearing octomap..."); + RCLCPP_INFO(moveit::get_logger(), "Clearing octomap..."); context_->planning_scene_monitor_->clearOctomap(); - RCLCPP_INFO(LOGGER, "Octomap cleared."); + RCLCPP_INFO(moveit::get_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 ff1bd1dac1f..869be200794 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,11 +41,10 @@ #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") { @@ -62,13 +61,9 @@ 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 */) { - RCLCPP_INFO(LOGGER, "Received request to cancel goal"); - return rclcpp_action::CancelResponse::ACCEPT; - }, + [](const std::shared_ptr& /* unused */) { return rclcpp_action::CancelResponse::ACCEPT; }, [this](const auto& goal) { executePathCallback(goal); }); } @@ -105,7 +100,7 @@ void MoveGroupExecuteTrajectoryAction::executePathCallback(const std::shared_ptr void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr& goal, std::shared_ptr& action_res) { - RCLCPP_INFO(LOGGER, "Execution request received"); + RCLCPP_INFO(moveit::get_logger(), "Execution request received"); context_->trajectory_execution_manager_->clear(); if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names)) @@ -129,7 +124,7 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptrerror_code.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED; } - RCLCPP_INFO_STREAM(LOGGER, "Execution completed: " << status.asString()); + RCLCPP_INFO_STREAM(moveit::get_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 704c533ae15..ef39fe09780 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,11 +40,10 @@ #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") { @@ -206,7 +205,7 @@ bool MoveGroupKinematicsService::computeFKService(const std::shared_ptrfk_link_names.empty()) { - RCLCPP_ERROR(LOGGER, "No links specified for FK request"); + RCLCPP_ERROR(moveit::get_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 b199bebc26f..9a59d99e319 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,13 +43,15 @@ #include #include #include +#include + +using moveit::get_logger; 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 @@ -66,11 +68,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(LOGGER, "Received request"); + RCLCPP_INFO(get_logger(), "MoveGroupMoveAction: Received request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, [this](const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(LOGGER, "Received request to cancel goal"); + RCLCPP_INFO(get_logger(), "MoveGroupMoveAction: Received request to cancel goal"); preemptMoveCallback(); return rclcpp_action::CancelResponse::ACCEPT; }, @@ -83,7 +85,7 @@ void MoveGroupMoveAction::initialize() void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr& goal) { goal_ = goal; - RCLCPP_INFO(LOGGER, "executing.."); + RCLCPP_INFO(get_logger(), "executing.."); setMoveState(PLANNING, goal_); // before we start planning, ensure that we have the latest robot state received... auto node = context_->moveit_cpp_->getNode(); @@ -95,9 +97,9 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptrget_goal()->planning_options.plan_only) { - 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."); + RCLCPP_WARN(get_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); } @@ -106,8 +108,8 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptrplanned_trajectory); // @todo: Response messages - RCLCPP_INFO_STREAM(LOGGER, getActionResultString(action_res->error_code, planned_trajectory_empty, - goal->get_goal()->planning_options.plan_only)); + RCLCPP_INFO_STREAM(get_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); @@ -129,8 +131,8 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr& goal, std::shared_ptr& action_res) { - RCLCPP_INFO(LOGGER, "Combined planning and execution request received for MoveGroup action. " - "Forwarding to planning and execution pipeline."); + RCLCPP_INFO(get_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)) { @@ -144,7 +146,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(LOGGER, "Goal constraints are already satisfied. No need to plan or execute any motions"); + RCLCPP_INFO(get_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; } @@ -173,7 +175,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt plan_execution::ExecutableMotionPlan plan; if (preempt_requested_) { - RCLCPP_INFO(LOGGER, "Preempt requested before the goal is planned and executed."); + RCLCPP_INFO(get_logger(), "Preempt requested before the goal is planned and executed."); action_res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED; return; } @@ -189,7 +191,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr& goal, std::shared_ptr& action_res) { - RCLCPP_INFO(LOGGER, "Planning request received for MoveGroup action. Forwarding to planning pipeline."); + RCLCPP_INFO(get_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_); @@ -201,7 +203,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptrerror_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED; return; } @@ -220,13 +222,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(LOGGER, "Generating a plan with planning pipeline failed."); + RCLCPP_ERROR(get_logger(), "Generating a plan with planning pipeline failed."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); + RCLCPP_ERROR(get_logger(), "Planning pipeline threw an exception: %s", ex.what()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } @@ -259,7 +261,7 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); + RCLCPP_ERROR(get_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 39cb03e05d4..327bb6dffc7 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(LOGGER, "Received new planning service request..."); + RCLCPP_INFO(moveit::get_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(LOGGER, "Generating a plan with planning pipeline failed."); + RCLCPP_ERROR(moveit::get_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(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); + RCLCPP_ERROR(moveit::get_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 06681b6180e..0fdc9fc0154 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,11 +42,10 @@ #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") { @@ -133,7 +132,7 @@ void TfPublisher::initialize() keep_running_ = true; - RCLCPP_INFO(LOGGER, "Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_); + RCLCPP_INFO(moveit::get_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 429168efe09..3024757f1b9 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -44,12 +44,11 @@ #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 @@ -110,7 +109,7 @@ class MoveGroupExe } } else - RCLCPP_ERROR(LOGGER, "No MoveGroup context created. Nothing will work."); + RCLCPP_ERROR(moveit::get_logger(), "No MoveGroup context created. Nothing will work."); } MoveGroupContextPtr getContext() @@ -128,7 +127,8 @@ class MoveGroupExe } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating plugin loader for move_group capabilities: " << ex.what()); + RCLCPP_FATAL_STREAM(moveit::get_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(LOGGER, + RCLCPP_ERROR_STREAM(moveit::get_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(LOGGER, "%s", ss.str().c_str()); + RCLCPP_INFO(moveit::get_logger(), "%s", ss.str().c_str()); } MoveGroupContextPtr context_; @@ -212,6 +212,7 @@ int main(int argc, char** argv) opt.allow_undeclared_parameters(true); opt.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("move_group", opt); + moveit::set_logger(nh->get_logger()); moveit_cpp::MoveItCpp::Options moveit_cpp_options(nh); // Prepare PlanningPipelineOptions @@ -221,7 +222,7 @@ int main(int argc, char** argv) { if (planning_pipeline_configs.empty()) { - RCLCPP_ERROR(LOGGER, "Failed to read parameter 'move_group.planning_pipelines'"); + RCLCPP_ERROR(moveit::get_logger(), "Failed to read parameter 'move_group.planning_pipelines'"); } else { @@ -240,7 +241,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(LOGGER, + RCLCPP_WARN(moveit::get_logger(), "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines", default_planning_pipeline.c_str()); default_planning_pipeline = ""; // reset invalid pipeline id @@ -249,7 +250,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(LOGGER, + RCLCPP_WARN(moveit::get_logger(), "MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default " "planning pipeline configuration"); } @@ -259,12 +260,13 @@ int main(int argc, char** argv) { if (!pipeline_names.empty()) { - RCLCPP_WARN(LOGGER, "Using default pipeline '%s'", pipeline_names[0].c_str()); + RCLCPP_WARN(moveit::get_logger(), "Using default pipeline '%s'", pipeline_names[0].c_str()); default_planning_pipeline = pipeline_names[0]; } else { - RCLCPP_WARN(LOGGER, "Falling back to using the the move_group node namespace (deprecated behavior)."); + RCLCPP_WARN(moveit::get_logger(), + "Falling back to using the the move_group node namespace (deprecated behavior)."); default_planning_pipeline = "move_group"; moveit_cpp_options.planning_pipeline_options.pipeline_names = { default_planning_pipeline }; moveit_cpp_options.planning_pipeline_options.parent_namespace = nh->get_effective_namespace(); @@ -292,11 +294,11 @@ int main(int argc, char** argv) debug = true; if (debug) { - RCLCPP_INFO(LOGGER, "MoveGroup debug mode is ON"); + RCLCPP_INFO(moveit::get_logger(), "MoveGroup debug mode is ON"); } else { - RCLCPP_INFO(LOGGER, "MoveGroup debug mode is OFF"); + RCLCPP_INFO(moveit::get_logger(), "MoveGroup debug mode is OFF"); } rclcpp::executors::MultiThreadedExecutor executor; @@ -306,7 +308,7 @@ int main(int argc, char** argv) bool monitor_dynamics; if (nh->get_parameter("monitor_dynamics", monitor_dynamics) && monitor_dynamics) { - RCLCPP_INFO(LOGGER, "MoveGroup monitors robot dynamics (higher load)"); + RCLCPP_INFO(moveit::get_logger(), "MoveGroup monitors robot dynamics (higher load)"); planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); } @@ -319,7 +321,7 @@ int main(int argc, char** argv) rclcpp::shutdown(); } else - RCLCPP_ERROR(LOGGER, "Planning scene not configured"); + RCLCPP_ERROR(moveit::get_logger(), "Planning scene not configured"); return 0; } diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index 886076297e7..6e7e02da7e8 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -39,12 +39,11 @@ #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; @@ -89,7 +88,8 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vector 1) - RCLCPP_ERROR_STREAM(LOGGER, "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!"); + RCLCPP_ERROR_STREAM(moveit::get_logger(), + "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!"); if (!trajectory.empty()) convertToMsg(trajectory[0].trajectory, first_state_msg, trajectory_msg); } @@ -100,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(LOGGER, + RCLCPP_WARN(moveit::get_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; @@ -112,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(LOGGER, + RCLCPP_WARN(moveit::get_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; @@ -200,7 +200,7 @@ bool move_group::MoveGroupCapability::performTransform(geometry_msgs::msg::PoseS } catch (tf2::TransformException& ex) { - RCLCPP_ERROR(LOGGER, "TF Problem: %s", ex.what()); + RCLCPP_ERROR(moveit::get_logger(), "TF Problem: %s", ex.what()); return false; } return true; @@ -220,12 +220,12 @@ move_group::MoveGroupCapability::resolvePlanningPipeline(const std::string& pipe try { auto pipeline = context_->moveit_cpp_->getPlanningPipelines().at(pipeline_id); - RCLCPP_INFO(LOGGER, "Using planning pipeline '%s'", pipeline_id.c_str()); + RCLCPP_INFO(moveit::get_logger(), "Using planning pipeline '%s'", pipeline_id.c_str()); return pipeline; } catch (const std::out_of_range&) { - RCLCPP_WARN(LOGGER, "Couldn't find requested planning pipeline '%s'", pipeline_id.c_str()); + RCLCPP_WARN(moveit::get_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 6c595893dd0..98152b45e59 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -39,8 +39,7 @@ #include #include #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_capabilities_base.move_group_context"); +#include move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, @@ -60,7 +59,7 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m else { RCLCPP_ERROR( - LOGGER, + moveit::get_logger(), "Failed to find default PlanningPipeline '%s' - please check MoveGroup's planning pipeline configuration.", default_planning_pipeline.c_str()); } @@ -86,13 +85,15 @@ bool move_group::MoveGroupContext::status() const const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager(); if (planner_interface) { - RCLCPP_INFO_STREAM(LOGGER, "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); - RCLCPP_INFO_STREAM(LOGGER, "MoveGroup context initialization complete"); + RCLCPP_INFO_STREAM(moveit::get_logger(), + "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); + RCLCPP_INFO_STREAM(moveit::get_logger(), "MoveGroup context initialization complete"); return true; } else { - RCLCPP_WARN_STREAM(LOGGER, "MoveGroup running was unable to load " << planning_pipeline_->getPlannerPluginName()); + RCLCPP_WARN_STREAM(moveit::get_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 237f3f715ac..ee99981c10a 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,20 +43,18 @@ #include #include #include +#include +using moveit::get_logger; 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::set_logger(demo_node->get_logger()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -90,7 +88,7 @@ int main(int argc, char* argv[]) std::chrono::seconds time_elapsed(0); auto start_time = std::chrono::steady_clock::now(); - RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + RCLCPP_INFO_STREAM(get_logger(), servo.getStatusMessage()); while (rclcpp::ok()) { const KinematicState joint_state = servo.getNextJointState(joint_jog); @@ -100,7 +98,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(LOGGER, "Timed out"); + RCLCPP_INFO_STREAM(get_logger(), "Timed out"); break; } else if (status != StatusCode::INVALID) @@ -110,6 +108,6 @@ int main(int argc, char* argv[]) rate.sleep(); } - RCLCPP_INFO(LOGGER, "Exiting demo."); + RCLCPP_INFO(get_logger(), "Exiting demo."); rclcpp::shutdown(); } diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index bfb5e92ad70..94b396f7c06 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -46,20 +46,18 @@ #include #include #include +#include +using moveit::get_logger; 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::set_logger(demo_node->get_logger()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -126,7 +124,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(LOGGER, servo.getStatusMessage()); + RCLCPP_INFO_STREAM(get_logger(), servo.getStatusMessage()); while (!stop_tracking && rclcpp::ok()) { @@ -148,12 +146,12 @@ int main(int argc, char* argv[]) command_rate.sleep(); } - RCLCPP_INFO_STREAM(LOGGER, "REACHED : " << stop_tracking); + RCLCPP_INFO_STREAM(get_logger(), "REACHED : " << stop_tracking); stop_tracking = true; if (tracker_thread.joinable()) tracker_thread.join(); - RCLCPP_INFO(LOGGER, "Exiting demo."); + RCLCPP_INFO(get_logger(), "Exiting demo."); rclcpp::shutdown(); } diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp index 3bc53ca9fd8..e9abc8594cd 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -45,20 +45,18 @@ #include #include #include +#include +using moveit::get_logger; 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::set_logger(demo_node->get_logger()); // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -94,7 +92,7 @@ int main(int argc, char* argv[]) std::chrono::seconds time_elapsed(0); auto start_time = std::chrono::steady_clock::now(); - RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + RCLCPP_INFO_STREAM(get_logger(), servo.getStatusMessage()); while (rclcpp::ok()) { const KinematicState joint_state = servo.getNextJointState(target_twist); @@ -104,7 +102,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(LOGGER, "Timed out"); + RCLCPP_INFO_STREAM(get_logger(), "Timed out"); break; } else if (status != StatusCode::INVALID) @@ -114,6 +112,6 @@ int main(int argc, char* argv[]) rate.sleep(); } - RCLCPP_INFO(LOGGER, "Exiting demo."); + RCLCPP_INFO(get_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 b193e439fa7..ec9ff168a26 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -53,6 +53,7 @@ #include #include #include +#include namespace moveit_servo { @@ -206,6 +207,7 @@ 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 3e343110222..c1d1869b966 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -39,11 +39,9 @@ #include #include +#include -namespace -{ -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); -} +using moveit::get_logger; namespace moveit_servo { @@ -62,11 +60,11 @@ void CollisionMonitor::start() if (!monitor_thread_.joinable()) { monitor_thread_ = std::thread(&CollisionMonitor::checkCollisions, this); - RCLCPP_INFO_STREAM(LOGGER, "Collision monitor started"); + RCLCPP_INFO_STREAM(get_logger(), "Collision monitor started"); } else { - RCLCPP_ERROR_STREAM(LOGGER, "Collision monitor could not be started"); + RCLCPP_ERROR_STREAM(get_logger(), "Collision monitor could not be started"); } } @@ -77,7 +75,7 @@ void CollisionMonitor::stop() { monitor_thread_.join(); } - RCLCPP_INFO_STREAM(LOGGER, "Collision monitor stopped"); + RCLCPP_INFO_STREAM(get_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 d32c3d427bb..edce2f24e60 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,6 +58,7 @@ namespace moveit_servo Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : node_(node) + , logger_(moveit::make_child_logger("servo")) , servo_param_listener_{ std::move(servo_param_listener) } , planning_scene_monitor_{ planning_scene_monitor } { @@ -65,14 +66,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); } @@ -91,12 +92,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 { @@ -107,7 +108,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() @@ -166,7 +167,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); } @@ -177,7 +178,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(); @@ -196,49 +197,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; @@ -258,8 +259,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; @@ -267,7 +268,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; @@ -314,7 +315,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) || @@ -376,7 +377,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) @@ -391,7 +392,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."); } } @@ -403,7 +404,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; @@ -473,7 +474,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 02dd61bce96..68c2613f1ff 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -40,11 +40,9 @@ #include #include +#include -namespace -{ -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); -} // namespace +using moveit::get_logger; namespace moveit_servo { @@ -69,11 +67,14 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) , new_twist_msg_{ false } , new_pose_msg_{ false } { + moveit::set_logger(node_->get_logger()); + if (!options.use_intra_process_comms()) { - 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"); + RCLCPP_WARN_STREAM(get_logger(), + "Intra-process communication is disabled, consider enabling it by adding: " + "\nextra_arguments=[{'use_intra_process_comms' : True}]\nto the Servo composable node " + "in the launch file"); } // Check if a realtime kernel is available @@ -81,16 +82,16 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) { if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority)) { - RCLCPP_INFO_STREAM(LOGGER, "Realtime kernel available, higher thread priority has been set."); + RCLCPP_INFO_STREAM(get_logger(), "Realtime kernel available, higher thread priority has been set."); } else { - RCLCPP_WARN_STREAM(LOGGER, "Could not enable FIFO RT scheduling policy."); + RCLCPP_WARN_STREAM(get_logger(), "Could not enable FIFO RT scheduling policy."); } } else { - RCLCPP_WARN_STREAM(LOGGER, "Realtime kernel is recommended for better performance."); + RCLCPP_WARN_STREAM(get_logger(), "Realtime kernel is recommended for better performance."); } std::shared_ptr servo_param_listener = @@ -177,7 +178,7 @@ void ServoNode::switchCommandType(const std::shared_ptrcommand_type << " requested"); + RCLCPP_WARN_STREAM(get_logger(), "Unknown command type " << request->command_type << " requested"); } response->success = (request->command_type == static_cast(servo_->getCommandType())); } @@ -220,7 +221,7 @@ std::optional ServoNode::processJointJogCommand() if (new_joint_jog_msg_) { next_joint_state = result.second; - RCLCPP_DEBUG_STREAM(LOGGER, "Joint jog command timed out. Halting to a stop."); + RCLCPP_DEBUG_STREAM(get_logger(), "Joint jog command timed out. Halting to a stop."); } } @@ -252,7 +253,7 @@ std::optional ServoNode::processTwistCommand() if (new_twist_msg_) { next_joint_state = result.second; - RCLCPP_DEBUG_STREAM(LOGGER, "Twist command timed out. Halting to a stop."); + RCLCPP_DEBUG_STREAM(get_logger(), "Twist command timed out. Halting to a stop."); } } @@ -281,7 +282,7 @@ std::optional ServoNode::processPoseCommand() if (new_pose_msg_) { next_joint_state = result.second; - RCLCPP_DEBUG_STREAM(LOGGER, "Pose command timed out. Halting to a stop."); + RCLCPP_DEBUG_STREAM(get_logger(), "Pose command timed out. Halting to a stop."); } } @@ -318,7 +319,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(LOGGER, "Command type has not been set, cannot accept input"); + RCLCPP_WARN_STREAM(get_logger(), "Command type has not been set, cannot accept input"); } if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID) && diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 9ddfbda8b00..bfa3d45da36 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -38,10 +38,12 @@ */ #include +#include + +using moveit::get_logger; 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 @@ -100,7 +102,7 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo } else { - RCLCPP_WARN_STREAM(LOGGER, "Invalid joint name: " << command.names[i]); + RCLCPP_WARN_STREAM(get_logger(), "Invalid joint name: " << command.names[i]); names_valid = false; break; @@ -120,13 +122,14 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo status = StatusCode::INVALID; if (!names_valid) { - 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."); + RCLCPP_WARN_STREAM(get_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(LOGGER, "Invalid velocity values in joint jog command"); + RCLCPP_WARN_STREAM(get_logger(), "Invalid velocity values in joint jog command"); } } @@ -179,7 +182,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: if (singularity_scaling_info.second != StatusCode::NO_WARNING) { status = singularity_scaling_info.second; - RCLCPP_WARN_STREAM(LOGGER, SERVO_STATUS_CODE_MAP.at(status)); + RCLCPP_WARN_STREAM(get_logger(), SERVO_STATUS_CODE_MAP.at(status)); joint_position_delta *= singularity_scaling_info.first; } } @@ -193,11 +196,11 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: status = StatusCode::INVALID; if (!valid_command) { - RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command."); + RCLCPP_WARN_STREAM(get_logger(), "Invalid twist command."); } if (!is_planning_frame) { - RCLCPP_WARN_STREAM(LOGGER, + RCLCPP_WARN_STREAM(get_logger(), "Command frame is: " << command.frame_id << ", expected: " << servo_params.planning_frame); } } @@ -243,11 +246,11 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co status = StatusCode::INVALID; if (!valid_command) { - RCLCPP_WARN_STREAM(LOGGER, "Invalid pose command."); + RCLCPP_WARN_STREAM(get_logger(), "Invalid pose command."); } if (!is_planning_frame) { - RCLCPP_WARN_STREAM(LOGGER, + RCLCPP_WARN_STREAM(get_logger(), "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame); } } @@ -276,7 +279,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt if (!ik_solver_supports_group) { status = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(LOGGER, "Loaded IK plugin does not support group " << joint_model_group->getName()); + RCLCPP_ERROR_STREAM(get_logger(), "Loaded IK plugin does not support group " << joint_model_group->getName()); } } @@ -307,7 +310,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt else { status = StatusCode::INVALID; - RCLCPP_WARN_STREAM(LOGGER, "Could not find IK solution for requested motion, got error code " << err.val); + RCLCPP_WARN_STREAM(get_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 cb817e14864..8b34da6d8a9 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,5 +329,7 @@ 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 9d85099bc71..d73e7c43a7d 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,6 +110,7 @@ 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 e3bb61c4c83..9778bf25df5 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -47,13 +47,10 @@ #include #include #include +#include namespace occupancy_map_monitor { -namespace -{ -static 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 } @@ -76,6 +73,7 @@ OccupancyMapMonitor::OccupancyMapMonitor(std::unique_ptr middl , debug_info_{ false } , mesh_handle_count_{ 0 } , active_{ false } + , logger_(moveit::make_child_logger("occupancy_map_monitor")) { if (middleware_handle_ == nullptr) { @@ -85,16 +83,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); @@ -107,7 +105,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; } @@ -120,7 +118,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; } @@ -179,7 +177,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) @@ -262,7 +260,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; } @@ -282,7 +280,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 { @@ -300,7 +298,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(); @@ -310,7 +308,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 4ba3e80daf5..005ecfab537 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,6 +36,7 @@ #include #include +#include #include #include @@ -48,15 +49,13 @@ namespace occupancy_map_monitor { -namespace -{ -static 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, {} } + : node_{ node } + , parameters_{ map_resolution, map_frame, {} } + , logger_(moveit::make_child_logger("occupancy_map_monitor")) { try { @@ -65,7 +64,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; } @@ -74,7 +73,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); } } @@ -84,18 +83,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) @@ -103,12 +102,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 @@ -131,8 +130,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; } @@ -141,7 +140,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 2a0ee448428..39e676f9d07 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #else @@ -56,8 +57,6 @@ #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) { @@ -74,7 +73,7 @@ static void publishOctomap(const rclcpp::Publisher:: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000, "Could not generate OctoMap message"); + RCLCPP_ERROR_THROTTLE(moveit::get_logger(), steady_clock, 1000, "Could not generate OctoMap message"); #pragma GCC diagnostic pop } } @@ -82,7 +81,7 @@ static void publishOctomap(const rclcpp::Publisher:: { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000, "Exception thrown while generating OctoMap message"); + RCLCPP_ERROR_THROTTLE(moveit::get_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 72b7c12e897..8fd409fc6d7 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( - LOGGER, steady_clock, 1000, + moveit::get_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(LOGGER, steady_clock, 1000, + RCLCPP_WARN_THROTTLE(moveit::get_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 ba32c93bf56..600648c0f29 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 @@ -107,5 +107,6 @@ 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 958bf35a6f8..4fd76592d22 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,6 +71,7 @@ DepthImageOctomapUpdater::DepthImageOctomapUpdater() , K2_(0.0) , K4_(0.0) , K5_(0.0) + , logger_(moveit::make_child_logger("depth_image_octomap_updater")) { } @@ -99,7 +100,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; } } @@ -184,7 +185,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; } @@ -199,7 +200,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; @@ -224,7 +225,7 @@ static const bool HOST_IS_BIG_ENDIAN = 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(); @@ -317,7 +318,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()); @@ -327,7 +328,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 } @@ -351,7 +352,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 } @@ -374,7 +375,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; @@ -572,7 +573,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(); @@ -591,7 +592,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(); @@ -599,6 +600,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 c2686a8a025..cea8f3064be 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,6 +41,7 @@ #include #include #include +#include namespace occupancy_map_monitor { @@ -87,5 +88,7 @@ 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 0ef67b85403..7cbd5006b00 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,6 +51,7 @@ LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const collision_detection::OccMapTree , process_model_cells_set_(nullptr) , update_thread_([this] { lazyUpdateThread(); }) , process_thread_([this] { processThread(); }) + , logger_(moveit::make_child_logger("lazy_free_space_updater")) { } @@ -72,7 +73,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_); @@ -98,7 +99,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; } @@ -124,7 +125,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())); @@ -175,7 +176,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(); @@ -194,12 +195,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; @@ -243,7 +244,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; @@ -265,7 +266,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 2176b9db7d9..723fcc24b80 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -43,6 +43,7 @@ #endif #include #include +#include #include #include #include @@ -53,8 +54,6 @@ 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) @@ -345,7 +344,7 @@ GLuint mesh_filter::GLRenderer::loadShaders(const string& vertex_source, const s glGetProgramInfoLog(program_id, info_log_length, nullptr, &program_error_message[0]); std::size_t l = strnlen(&program_error_message[0], program_error_message.size()); if (l > 0) - RCLCPP_ERROR(LOGGER, "%s\n", &program_error_message[0]); + RCLCPP_ERROR(moveit::get_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 fafbf8a5941..44f72bd3f1a 100644 --- a/moveit_ros/perception/point_containment_filter/CMakeLists.txt +++ b/moveit_ros/perception/point_containment_filter/CMakeLists.txt @@ -6,6 +6,7 @@ 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 55ac24d5516..c5dff8e41eb 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -39,8 +39,7 @@ #include #include #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.shape_mask"); +#include point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) : transform_callback_(transform_callback), next_handle_(1), min_handle_(1) @@ -81,7 +80,8 @@ 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(LOGGER, "Internal error in management of bodies in ShapeMask. This is a serious error."); + RCLCPP_ERROR(moveit::get_logger(), + "Internal error in management of bodies in ShapeMask. This is a serious error."); used_handles_[next_handle_] = insert_op.first; } else @@ -114,7 +114,7 @@ void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle) min_handle_ = handle; } else - RCLCPP_ERROR(LOGGER, "Unable to remove shape handle %u", handle); + RCLCPP_ERROR(moveit::get_logger(), "Unable to remove shape handle %u", handle); } void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::msg::PointCloud2& data_in, @@ -141,11 +141,12 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::msg { if (!it->body) { - RCLCPP_ERROR_STREAM(LOGGER, "Missing transform for shape with handle " << it->handle << " without a body"); + RCLCPP_ERROR_STREAM(moveit::get_logger(), + "Missing transform for shape with handle " << it->handle << " without a body"); } else { - RCLCPP_ERROR_STREAM(LOGGER, + RCLCPP_ERROR_STREAM(moveit::get_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 79f00bf86f3..ea1d0c27355 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 @@ -102,5 +102,7 @@ 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 a77cc7fa6cd..e22d0722087 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,6 +58,7 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater() , max_update_rate_(0) , point_cloud_subscriber_(nullptr) , point_cloud_filter_(nullptr) + , logger_(moveit::make_child_logger("pointcloud_octomap_updater")) { } @@ -118,14 +119,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()); } } @@ -151,7 +152,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; } @@ -179,7 +180,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) @@ -211,7 +212,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; } } @@ -368,10 +369,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 bfc316aa181..1f56067e12e 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,6 +158,8 @@ 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 e63ac1c5f25..ff966d90a1b 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -40,6 +40,7 @@ #include #include #include +#include // OpenCV #include #include @@ -63,11 +64,10 @@ 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) + : planning_scene_(planning_scene), node_handle_(node), logger_(moveit::make_child_logger("semantic_world")) { 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 e3b1958dd55..93c67a8d5d9 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,8 +43,13 @@ 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 dd62628b362..b8c1db0a790 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,18 +33,22 @@ *********************************************************************/ #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::make_child_logger("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; } @@ -73,7 +77,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 b49c616cd2f..f6ab0734965 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,15 +41,16 @@ #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) + Helper(const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) + : node_(node), logger_(moveit::make_child_logger("constraint_sampler_manager_loader")) { if (node_->has_parameter("constraint_samplers")) { @@ -63,7 +64,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(" "); @@ -75,11 +76,12 @@ 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()); } } } @@ -89,6 +91,7 @@ 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 30a1793966c..a2ae8bd374a 100644 --- a/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp +++ b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp @@ -42,12 +42,14 @@ #include #include #include +#include int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared("list_planning_adapter_plugins"); + moveit::set_logger(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 af4f6c88074..89d56d47675 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,6 +40,7 @@ #include #include #include +#include namespace kinematics_plugin_loader { @@ -56,7 +57,7 @@ class KinematicsPluginLoader well as used to read the SRDF document when needed. */ KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node, const std::string& robot_description = "robot_description") - : node_(node), robot_description_(robot_description) + : node_(node), robot_description_(robot_description), logger_(moveit::make_child_logger("kinematics_plugin_loader")) { } @@ -89,5 +90,6 @@ 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 05465811654..db723955d54 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,11 +61,12 @@ 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 std::map& search_res, const rclcpp::Logger& logger) : node_(node) , robot_description_(robot_description) , possible_kinematics_solvers_(possible_kinematics_solvers) , search_res_(search_res) + , logger_(logger) { try { @@ -75,7 +76,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()); } } @@ -88,7 +89,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()); @@ -98,7 +99,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 @@ -106,7 +107,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; } @@ -116,23 +117,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() : @@ -161,14 +162,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()); @@ -177,14 +178,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; } @@ -208,7 +209,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)); } } @@ -222,6 +223,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl std::map instances_; std::mutex lock_; std::mutex cache_lock_; + rclcpp::Logger logger_; }; void KinematicsPluginLoader::status() const @@ -232,7 +234,7 @@ void KinematicsPluginLoader::status() const } else { - RCLCPP_INFO(LOGGER, "Loader function was never required"); + RCLCPP_INFO(logger_, "Loader function was never required"); } } @@ -240,7 +242,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; @@ -251,7 +253,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) { @@ -265,7 +267,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; } @@ -273,26 +275,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); + loader_ = std::make_shared(node_, robot_description_, possible_kinematics_solvers, search_res, + logger_); } 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 a9d6fe3f9d7..f3c6edaa7ba 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,6 +183,8 @@ 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 4d134810c02..88668e1f01e 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,6 +239,7 @@ 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 f8ef3b7fbd0..cebbe8e6f59 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) +MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options) + : node_(node), logger_(moveit::make_child_logger("moveit_cpp")) { // 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 d161be7f834..e288b2e0195 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -42,19 +42,22 @@ #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) + : node_(moveit_cpp->getNode()) + , moveit_cpp_(moveit_cpp) + , group_name_(group_name) + , logger_(moveit::make_child_logger("planning_component")) { 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); } } @@ -66,7 +69,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); } } @@ -79,7 +82,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; @@ -111,7 +114,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; } @@ -119,7 +122,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; } @@ -156,7 +159,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; } @@ -164,7 +167,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; } @@ -201,7 +204,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 @@ -213,13 +216,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); } @@ -248,7 +251,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()); @@ -311,7 +314,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 4d63f066b73..5f2d9dd80bb 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,6 +42,7 @@ #include #include #include +#include #include @@ -182,6 +183,8 @@ 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 378bd1ec826..d02af7c74d0 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,7 +80,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.plan_executi 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) + : node_(node) + , planning_scene_monitor_(planning_scene_monitor) + , trajectory_execution_manager_(trajectory_execution) + , logger_(moveit::make_child_logger("add_time_optimal_parameterization")) { if (!trajectory_execution_manager_) { @@ -159,7 +162,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_(); @@ -229,10 +232,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(node_->get_logger(), "Done waiting"); + RCLCPP_INFO(logger_, "Done waiting"); } } @@ -244,7 +247,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; } @@ -253,11 +256,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::error_code_to_string(plan.error_code).c_str()); } } @@ -294,7 +297,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 @@ -334,7 +337,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; } @@ -399,7 +402,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; @@ -438,7 +441,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; @@ -453,19 +456,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(); } @@ -529,18 +532,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; } @@ -554,7 +557,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 b77d9ab3887..089e93cb670 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,6 +36,7 @@ #include #include +#include using namespace std::chrono_literals; @@ -45,6 +46,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("display_random_state"); + moveit::set_logger(node->get_logger()); bool valid = false; bool invalid = false; @@ -79,7 +81,7 @@ int main(int argc, char** argv) { if (!psm.getPlanningScene()) { - RCLCPP_ERROR(LOGGER, "Planning scene did not load properly, exiting..."); + RCLCPP_ERROR(moveit::get_logger(), "Planning scene did not load properly, exiting..."); break; } diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index add1a93c5df..a3196532ddd 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::get_logger; 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(LOGGER, "Starting thread %u", id); + RCLCPP_INFO(get_logger(), "Starting thread %u", id); rclcpp::Clock clock(RCL_ROS_TIME); collision_detection::CollisionRequest req; rclcpp::Time start = clock.now(); @@ -59,13 +59,15 @@ void runCollisionDetection(unsigned int id, unsigned int trials, const planning_ scene.checkCollision(req, res, state); } double duration = (clock.now() - start).seconds(); - RCLCPP_INFO(LOGGER, "Thread %u performed %lf collision checks per second", id, static_cast(trials) / duration); + RCLCPP_INFO(get_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::set_logger(node->get_logger()); unsigned int nthreads = 2; unsigned int trials = 10000; @@ -104,7 +106,7 @@ int main(int argc, char** argv) rclcpp::sleep_for(500ms); std::vector states; - RCLCPP_INFO(LOGGER, "Sampling %u valid states...", nthreads); + RCLCPP_INFO(get_logger(), "Sampling %u valid states...", nthreads); for (unsigned int i = 0; i < nthreads; ++i) { // sample a valid state @@ -138,7 +140,7 @@ int main(int argc, char** argv) } } else - RCLCPP_ERROR(LOGGER, "Planning scene not configured"); + RCLCPP_ERROR(get_logger(), "Planning scene not configured"); return 0; } diff --git a/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp b/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp index dd7726a66af..affabfaa6ea 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,6 +38,7 @@ #include #include #include +#include static const std::string ROBOT_DESCRIPTION = "robot_description"; @@ -47,6 +48,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("print_model_info_to_console"); + moveit::set_logger(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 463c397308f..9109bcaafdd 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,26 +43,27 @@ #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::set_logger(node->get_logger()); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); - RCLCPP_INFO_STREAM(LOGGER, "Getting planning scene info to print"); + RCLCPP_INFO_STREAM(moveit::get_logger(), "Getting planning scene info to print"); // Create planning scene monitor planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR_STREAM(LOGGER, "Planning scene not configured"); + RCLCPP_ERROR_STREAM(moveit::get_logger(), "Planning scene not configured"); return 1; } diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index cfe6719ded8..b80b6f1b429 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,6 +42,7 @@ #include #include #include +#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #else @@ -50,12 +51,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::set_logger(node->get_logger()); // decide whether to publish the full scene bool full_scene = false; @@ -100,7 +101,7 @@ int main(int argc, char** argv) std::ifstream f(argv[filename_index]); if (ps.loadGeometryFromStream(f)) { - RCLCPP_INFO(LOGGER, "Publishing geometry from '%s' ...", argv[filename_index]); + RCLCPP_INFO(moveit::get_logger(), "Publishing geometry from '%s' ...", argv[filename_index]); moveit_msgs::msg::PlanningScene ps_msg; ps.getPlanningSceneMsg(ps_msg); ps_msg.is_diff = true; @@ -123,7 +124,7 @@ int main(int argc, char** argv) } else { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(moveit::get_logger(), "A filename was expected as argument. That file should be a text representation of the geometry in a " "planning scene."); } diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index 5d365cd99d5..bd7bc188a83 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,6 +40,7 @@ #include #include #include +#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #else @@ -55,6 +56,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("visualize_robot_collision_volume"); + moveit::set_logger(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 96c0c0ab219..faf93daa446 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,6 +186,8 @@ 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 24f31c7cb62..e9b31133995 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) + : active_{ false } + , node_(node) + , parameter_namespace_(parameter_namespace) + , robot_model_(model) + , logger_(moveit::make_child_logger("planning_pipeline")) { auto param_listener = planning_pipeline_parameters::ParamListener(node, parameter_namespace); const auto params = param_listener.get_params(); @@ -81,6 +81,7 @@ 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::make_child_logger("planning_pipeline")) { configure(); } @@ -103,7 +104,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; } @@ -121,12 +122,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()); @@ -145,7 +146,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; } @@ -160,7 +161,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; } @@ -177,14 +178,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."); } } @@ -220,7 +221,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 @@ -232,7 +233,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; @@ -244,7 +245,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; @@ -279,7 +280,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 { @@ -292,10 +293,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) @@ -320,18 +321,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); } @@ -364,19 +365,20 @@ 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 af37378281a..c8aaf9ed511 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,6 +35,7 @@ /* Author: Sebastian Jahr */ #include +#include #include @@ -42,7 +43,12 @@ namespace moveit { namespace planning_pipeline_interfaces { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.::planning_interface.planning_pipeline_interfaces"); + +rclcpp::Logger get_logger() +{ + static rclcpp::Logger logger = moveit::make_child_logger("planning_pipeline_interfaces"); + return logger; +} ::planning_interface::MotionPlanResponse planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_plan_request, @@ -53,7 +59,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(LOGGER, "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); + RCLCPP_ERROR(get_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; } @@ -82,7 +88,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(LOGGER, + RCLCPP_WARN(get_logger(), "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " "hardware ('%d')", motion_plan_requests.size(), hardware_concurrency); @@ -100,7 +106,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::exception& e) { - RCLCPP_ERROR(LOGGER, "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); + RCLCPP_ERROR(get_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; } @@ -112,7 +118,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe if (stopping_criterion_callback(plan_responses_container, motion_plan_requests)) { // Terminate planning pipelines - RCLCPP_INFO(LOGGER, "Stopping criterion met: Terminating planning pipelines that are still active"); + RCLCPP_INFO(get_logger(), "Stopping criterion met: Terminating planning pipelines that are still active"); for (const auto& request : motion_plan_requests) { try @@ -125,7 +131,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::out_of_range&) { - RCLCPP_WARN(LOGGER, "Cannot terminate pipeline '%s' because no pipeline with that name exists", + RCLCPP_WARN(get_logger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists", request.pipeline_id.c_str()); } } @@ -168,7 +174,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, // Check if pipeline already exists if (planning_pipelines.count(planning_pipeline_name) > 0) { - RCLCPP_WARN(LOGGER, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_WARN(get_logger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } @@ -178,7 +184,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, if (!pipeline->getPlannerManager()) { - RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_ERROR(get_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 2e3638397b4..9e17b0daf0c 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,8 +45,6 @@ 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 677494212c0..a94fc412a9f 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,6 +37,7 @@ #include #include #include +#include #include @@ -44,12 +45,14 @@ 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::make_child_logger("add_time_optimal_parameterization")) + { + } + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -68,12 +71,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; } } @@ -83,6 +86,7 @@ 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 ae755e83204..93c7d832cc8 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,17 +52,21 @@ #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::make_child_logger("fix_start_state_bounds")) + { + } + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -78,7 +82,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(); @@ -150,7 +154,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 @@ -167,7 +171,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)", @@ -209,6 +213,7 @@ 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 b58196d0597..b526d9f09e8 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,18 +48,22 @@ #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::make_child_logger("fix_start_state_collision")) + { + } + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -75,7 +79,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(); @@ -95,11 +99,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); @@ -127,7 +131,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); } } @@ -157,7 +161,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); @@ -169,11 +173,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); } @@ -181,6 +185,7 @@ 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 3453dd0ec11..e7e15534b86 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,6 +59,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter { public: + FixStartStatePathConstraints() : logger_(moveit::make_child_logger("fix_start_state_path_constraints")) + { + } + void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } @@ -72,7 +76,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(); @@ -82,9 +86,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); @@ -101,7 +105,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); @@ -134,7 +138,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 @@ -142,10 +146,13 @@ 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 72c1bd924dc..ce3554372f1 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,18 +42,22 @@ #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::make_child_logger("fix_workspace_bounds")) + { + } + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { param_listener_ = @@ -69,13 +73,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(); @@ -94,6 +98,7 @@ 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 212c69db8a8..982797d2024 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,6 +37,7 @@ #include #include #include +#include namespace default_planner_request_adapters { @@ -45,6 +46,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.resolve_cons class ResolveConstraintFrames : public planning_request_adapter::PlanningRequestAdapter { public: + ResolveConstraintFrames() : logger_(moveit::make_child_logger("resolve_constraint_frames")) + { + } + void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } @@ -58,7 +63,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) @@ -67,6 +72,9 @@ 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 4b0a16e51e1..0e0c873de66 100644 --- a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp +++ b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp @@ -51,8 +51,6 @@ #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) @@ -93,7 +91,7 @@ void sendKnife(const rclcpp::Node::SharedPtr& node) pub_aco->publish(aco); rclcpp::sleep_for(1s); pub_aco->publish(aco); - RCLCPP_INFO(LOGGER, "Object published."); + RCLCPP_INFO(node->get_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 1b4c9b4995c..b1ff502d9c1 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,6 +342,8 @@ 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 b9a7a044159..15687b08f3a 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,6 +592,8 @@ 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 d5e55feadc6..9975093c069 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,5 +140,7 @@ 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 ed792a0b91c..649b6c1386d 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,6 +36,7 @@ #include #include +#include #include #include @@ -48,11 +49,6 @@ namespace planning_scene_monitor { using namespace std::chrono_literals; -namespace -{ -static 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) @@ -64,6 +60,7 @@ CurrentStateMonitor::CurrentStateMonitor(std::unique_ptr::epsilon()) , use_sim_time_(use_sim_time) + , logger_(moveit::make_child_logger("current_state_monitor")) { robot_state_.setToDefaultValues(); } @@ -155,7 +152,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 { @@ -172,7 +169,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()); } } @@ -190,7 +187,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; } } @@ -210,12 +207,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; @@ -250,7 +247,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); @@ -264,7 +261,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!", @@ -273,7 +270,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; } } @@ -329,7 +326,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 @@ -441,7 +438,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", @@ -511,7 +508,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 4e53f8756fb..2359cbc75dc 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,6 +39,7 @@ #include #include #include +#include #include #include @@ -53,8 +54,6 @@ #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"; @@ -97,6 +96,7 @@ PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, , dt_state_update_(0.0) , shape_transform_cache_lookup_wait_time_(0, 0) , rm_loader_(rm_loader) + , logger_(moveit::make_child_logger("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,16 +1104,17 @@ 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; } @@ -1150,7 +1151,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()) { @@ -1158,7 +1159,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()); } } @@ -1166,7 +1167,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(); } } @@ -1233,7 +1234,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; } @@ -1245,8 +1246,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()) @@ -1254,7 +1255,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()) @@ -1264,7 +1265,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 @@ -1292,12 +1293,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_) @@ -1338,12 +1339,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() @@ -1405,7 +1406,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)); } } @@ -1413,7 +1414,7 @@ void PlanningSceneMonitor::stateUpdateTimerCallback() if (update) { updateSceneWithCurrentState(); - RCLCPP_DEBUG(LOGGER, "performPendingStateUpdate done"); + RCLCPP_DEBUG(logger_, "performPendingStateUpdate done"); } } } @@ -1465,7 +1466,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(); @@ -1484,7 +1485,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 } @@ -1492,7 +1493,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 } @@ -1502,7 +1503,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 } @@ -1524,7 +1525,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_); } @@ -1546,7 +1547,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; @@ -1601,13 +1602,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; // } @@ -1616,7 +1617,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"]), @@ -1652,7 +1653,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 b3e1bd21cb6..ebef4a3876c 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -39,8 +39,7 @@ #include #include #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_scene_monitor.trajectory_monitor"); +#include planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency) @@ -56,6 +55,7 @@ planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor( , middleware_handle_(std::move(middleware_handle)) , sampling_frequency_(sampling_frequency) , trajectory_(current_state_monitor_->getRobotModel(), "") + , logger_(moveit::make_child_logger("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 85d1fa28aa6..dae75230397 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,5 +133,7 @@ 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 debae970c5f..4c4a568245b 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -55,11 +56,10 @@ 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) + : ros_name_(ros_name), logger_(moveit::make_child_logger("rdf_loader")) { 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) + : urdf_string_(urdf_string), srdf_string_(srdf_string), logger_(moveit::make_child_logger("rdf_loader")) { 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(LOGGER, "Path is empty"); + RCLCPP_ERROR(moveit::get_logger(), "Path is empty"); return false; } if (!std::filesystem::exists(path)) { - RCLCPP_ERROR(LOGGER, "File does not exist"); + RCLCPP_ERROR(moveit::get_logger(), "File does not exist"); return false; } std::ifstream stream(path.c_str()); if (!stream.good()) { - RCLCPP_ERROR(LOGGER, "Unable to load path"); + RCLCPP_ERROR(moveit::get_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(LOGGER, "Path is empty"); + RCLCPP_ERROR(moveit::get_logger(), "Path is empty"); return false; } if (!std::filesystem::exists(path)) { - RCLCPP_ERROR(LOGGER, "File does not exist"); + RCLCPP_ERROR(moveit::get_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(LOGGER, "Unable to load path"); + RCLCPP_ERROR(moveit::get_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(LOGGER, "ament_index_cpp: %s", e.what()); + RCLCPP_ERROR(moveit::get_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 ba2e447354a..8b7c54220dd 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,5 +133,6 @@ 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 2c2e04bc042..1e67811aa91 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,21 +42,22 @@ #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) + : node_(node), logger_(moveit::make_child_logger("robot_model_loader")) { 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) +RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const Options& opt) + : node_(node), logger_(moveit::make_child_logger("robot_model_loader")) { configure(opt); } @@ -75,23 +76,23 @@ RobotModelLoader::~RobotModelLoader() namespace { -bool canSpecifyPosition(const moveit::core::JointModel* jmodel, const unsigned int index) +bool canSpecifyPosition(const moveit::core::JointModel* jmodel, const unsigned int index, const rclcpp::Logger& logger) { 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 { @@ -142,7 +143,7 @@ void RobotModelLoader::configure(const Options& opt) double max_position; if (node_->get_parameter(param_name, max_position)) { - if (canSpecifyPosition(joint_model, joint_id)) + if (canSpecifyPosition(joint_model, joint_id, logger_)) { joint_limit[joint_id].has_position_limits = true; joint_limit[joint_id].max_position = max_position; @@ -157,7 +158,7 @@ void RobotModelLoader::configure(const Options& opt) double min_position; if (node_->get_parameter(param_name, min_position)) { - if (canSpecifyPosition(joint_model, joint_id)) + if (canSpecifyPosition(joint_model, joint_id, logger_)) { joint_limit[joint_id].has_position_limits = true; joint_limit[joint_id].min_position = min_position; @@ -202,7 +203,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()); } } @@ -217,7 +218,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()); } } @@ -232,14 +233,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); @@ -249,7 +250,7 @@ void RobotModelLoader::configure(const Options& opt) if (model_ && opt.load_kinematics_solvers) loadKinematicsSolvers(); - RCLCPP_DEBUG(node_->get_logger(), "Loaded kinematic model in %f seconds", (clock.now() - start).seconds()); + RCLCPP_DEBUG(logger_, "Loaded kinematic model in %f seconds", (clock.now() - start).seconds()); } void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader) @@ -271,9 +272,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) @@ -295,13 +296,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 e1ae1af1b1f..b76aaf6fb37 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,6 +278,7 @@ 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 b4b89168ff7..fdfce995c79 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), robot_model_(robot_model), csm_(csm) + : node_(node), logger_(moveit::make_child_logger("trajectory_execution_manager")), robot_model_(robot_model), csm_(csm) { if (!node_->get_parameter("moveit_manage_controllers", manage_controllers_)) manage_controllers_ = false; @@ -66,7 +66,11 @@ TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::Share const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers) - : node_(node), robot_model_(robot_model), csm_(csm), manage_controllers_(manage_controllers) + : node_(node) + , logger_(moveit::make_child_logger("trajectory_execution_manager")) + , robot_model_(robot_model) + , csm_(csm) + , manage_controllers_(manage_controllers) { initialize(); } @@ -106,7 +110,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; } @@ -120,16 +124,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; } } @@ -137,13 +141,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()) @@ -172,7 +176,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()); } } } @@ -199,11 +203,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) { @@ -294,13 +298,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); } @@ -342,7 +346,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; } @@ -359,7 +363,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; @@ -424,7 +428,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."); } } @@ -437,7 +441,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()); } } @@ -448,13 +452,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) @@ -482,7 +486,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()); } @@ -714,7 +718,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; @@ -724,7 +728,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()) { @@ -837,12 +841,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; } @@ -855,7 +859,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; } @@ -864,7 +868,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; } @@ -875,7 +879,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); @@ -891,7 +895,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; } @@ -901,7 +905,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; } @@ -917,10 +921,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; } } @@ -959,7 +963,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; } @@ -1011,7 +1015,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; } } @@ -1025,7 +1029,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; @@ -1040,7 +1044,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; } @@ -1061,7 +1065,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()); } } } @@ -1083,7 +1087,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_); @@ -1166,7 +1170,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, @@ -1181,7 +1185,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; @@ -1204,7 +1208,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(); @@ -1251,14 +1255,15 @@ 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; @@ -1273,7 +1278,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) { @@ -1285,13 +1290,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; @@ -1393,7 +1398,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()); @@ -1419,8 +1424,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; } @@ -1441,7 +1446,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; } @@ -1452,7 +1457,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; } @@ -1469,7 +1474,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(); @@ -1600,12 +1605,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_) @@ -1619,7 +1624,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 ec50305b171..38ec463c9e8 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,6 +47,7 @@ #include #include #include +#include #include #include @@ -943,6 +944,7 @@ 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 09e64e7db48..a193d4b1c6b 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,6 +57,7 @@ #include #include #include +#include #include #include @@ -74,7 +75,6 @@ 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), tf_buffer_(tf_buffer) + : opt_(opt), node_(node), logger_(moveit::make_child_logger("move_group_interface")), tf_buffer_(tf_buffer) { // We have no control on how the passed node is getting executed. To make sure MGI is functional, we're creating // our own callback group which is managed in a separate callback thread @@ -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), qos_default(), 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,8 +368,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (target_value > 1.0) { - RCLCPP_WARN(rclcpp::get_logger("move_group_interface"), "Limiting max_%s (%.2f) to 1.0.", factor_name, - target_value); + RCLCPP_WARN(logger_, "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value); variable = 1.0; } else if (target_value <= 0.0) @@ -378,8 +377,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl fallback_value); if (target_value < 0.0) { - RCLCPP_WARN(rclcpp::get_logger("move_group_interface"), "max_%s < 0.0! Setting to default: %.2f.", factor_name, - variable); + RCLCPP_WARN(logger_, "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable); } } else @@ -466,7 +464,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; } @@ -517,7 +515,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 @@ -551,7 +549,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; } @@ -568,7 +566,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; } @@ -601,7 +599,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; } @@ -617,7 +615,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; } @@ -627,7 +625,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; } @@ -639,10 +637,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); @@ -662,10 +660,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) { @@ -676,16 +674,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; } }; @@ -700,14 +698,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; } @@ -716,7 +714,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; } @@ -739,10 +737,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) { @@ -753,16 +751,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; } }; @@ -778,7 +776,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; } @@ -788,7 +786,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; } @@ -802,10 +800,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) { @@ -816,16 +814,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; } }; @@ -846,7 +844,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; } @@ -921,7 +919,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; @@ -1065,7 +1063,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_; @@ -1251,13 +1249,14 @@ 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_; @@ -1320,6 +1319,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group_name, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) + : logger_(moveit::make_child_logger("move_group_interface")) { if (!rclcpp::ok()) throw std::runtime_error("ROS does not seem to be running"); @@ -1330,6 +1330,7 @@ MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, cons MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) + : logger_(moveit::make_child_logger("move_group_interface")) { impl_ = new MoveGroupInterfaceImpl(node, opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); } @@ -1340,7 +1341,7 @@ MoveGroupInterface::~MoveGroupInterface() } MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) noexcept - : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_) + : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_), logger_(other.logger_) { other.impl_ = nullptr; } @@ -1351,6 +1352,7 @@ 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; } @@ -1618,7 +1620,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; @@ -1638,7 +1640,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; } } @@ -1653,9 +1655,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); @@ -1670,8 +1672,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; } } @@ -1686,7 +1688,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(); @@ -1694,8 +1696,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; } } @@ -1728,7 +1730,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; } @@ -1876,7 +1878,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 { @@ -2176,18 +2178,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; } } @@ -2195,18 +2197,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; } } @@ -2215,11 +2217,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 67da194b796..3d9c79d5a84 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,12 +41,14 @@ #include #include #include +#include + +using moveit::get_logger; namespace moveit { namespace planning_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_scene_interface.planning_scene_interface"); class PlanningSceneInterface::PlanningSceneInterfaceImpl { @@ -58,6 +60,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl "__node:=" + std::string("planning_scene_interface_") + std::to_string(reinterpret_cast(this)) }); node_ = rclcpp::Node::make_shared("_", ns, options); + moveit::set_logger(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); @@ -112,7 +115,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(LOGGER, "Could not call planning scene service to get object names"); + RCLCPP_WARN(get_logger(), "Could not call planning scene service to get object names"); return result; } response = res.get(); @@ -185,7 +188,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(LOGGER, "Could not call planning scene service to get object geometries"); + RCLCPP_WARN(get_logger(), "Could not call planning scene service to get object geometries"); return result; } response = res.get(); @@ -211,7 +214,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(LOGGER, "Could not call planning scene service to get attached object geometries"); + RCLCPP_WARN(get_logger(), "Could not call planning scene service to get attached object geometries"); return result; } response = res.get(); @@ -237,7 +240,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(LOGGER, "Failed to call ApplyPlanningScene service"); + RCLCPP_WARN(get_logger(), "Failed to call ApplyPlanningScene service"); } response = res.get(); return response->success; @@ -289,7 +292,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl srv->wait_for_service(std::chrono::duration_cast(d)); if (!srv->service_is_ready()) { - RCLCPP_WARN_STREAM(LOGGER, "service '" << srv->get_service_name() << "' not advertised yet. Continue waiting..."); + RCLCPP_WARN_STREAM(get_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 0ae3ae20413..31bf726f21b 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,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -232,6 +233,7 @@ 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 2422875cc46..0d4578b418a 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -54,7 +55,6 @@ 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(LOGGER, "Error transforming from frame '%s' to frame '%s'", tpose.header.frame_id.c_str(), - planning_frame_.c_str()); + RCLCPP_ERROR(moveit::get_logger(), "Error transforming from frame '%s' to frame '%s'", + tpose.header.frame_id.c_str(), planning_frame_.c_str()); return false; } } else { - RCLCPP_ERROR(LOGGER, "Cannot transform from frame '%s' to frame '%s' (no TF instance provided)", + RCLCPP_ERROR(moveit::get_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 ea03b441191..e4353ff4ed3 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -36,8 +36,7 @@ #include #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.kinematic_options"); +#include robot_interaction::KinematicOptions::KinematicOptions() : timeout_seconds_(0.0) // 0.0 = use default timeout { @@ -52,7 +51,7 @@ bool robot_interaction::KinematicOptions::setStateFromIK(moveit::core::RobotStat const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group); if (!jmg) { - RCLCPP_ERROR(LOGGER, "No getJointModelGroup('%s') found", group.c_str()); + RCLCPP_ERROR(moveit::get_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 e85d54cbfd3..7d3ac52f787 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -55,7 +56,6 @@ 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,10 +63,12 @@ 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), kinematic_options_map_(std::make_shared()) + : robot_model_(robot_model) + , node_(node) + , logger_(moveit::make_child_logger("moveit_ros_robot_interaction")) + , 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 @@ -95,7 +97,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()); @@ -196,7 +198,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); @@ -282,14 +284,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; } @@ -353,7 +355,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) @@ -485,7 +487,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); } } @@ -530,7 +532,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) { @@ -558,7 +560,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; } @@ -739,7 +741,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; } @@ -747,7 +749,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; } @@ -768,12 +770,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()); @@ -782,7 +784,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; } @@ -791,7 +793,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; } @@ -810,7 +812,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(); } @@ -826,7 +828,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(); } @@ -841,19 +843,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 20190830d0f..6288c349ab4 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,6 +58,7 @@ #include #include #include +#include #endif #include @@ -325,6 +326,7 @@ 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 7dd4ce0ee66..ac479a558b6 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(LOGGER, "Group [%s] not found in the robot model.", group.c_str()); + RCLCPP_ERROR(moveit::get_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 af37922f46f..9d450a8b1d4 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,6 +41,7 @@ #include #include #include +#include #include @@ -60,16 +61,20 @@ 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()), first_time_(true) + : QWidget(parent) + , planning_display_(pdisplay) + , context_(context) + , ui_(new Ui::MotionPlanningUI()) + , logger_(moveit::make_child_logger("motion_planning_frame")) + , 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(); @@ -211,7 +216,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(); } } @@ -233,7 +238,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()); // } } @@ -376,10 +381,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()); @@ -401,7 +406,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_) @@ -598,7 +603,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 f62ad223430..497f2655fda 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,7 +49,6 @@ namespace moveit_rviz_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_context"); void MotionPlanningFrame::databaseConnectButtonClicked() { @@ -112,7 +111,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_) { @@ -143,7 +142,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 3614f102cc1..3157891ec22 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,7 +45,6 @@ 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 d5423876047..5c20b487767 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,7 +47,6 @@ namespace moveit_rviz_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_manipulation"); /////////////// Object Detection /////////////////////// void MotionPlanningFrame::detectObjectsButtonClicked() @@ -104,7 +103,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); } @@ -113,7 +112,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(); @@ -144,7 +143,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; } } @@ -154,7 +153,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; } } @@ -192,7 +191,7 @@ void MotionPlanningFrame::updateDetectedObjectsList(const std::vectoraddBackgroundJob([this] { publishTables(); }, "publish tables"); } @@ -209,7 +208,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(); @@ -241,7 +240,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); @@ -265,14 +264,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(); @@ -288,7 +287,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 @@ -297,7 +296,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"); } @@ -314,7 +313,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; } @@ -325,7 +324,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); @@ -334,7 +333,7 @@ void MotionPlanningFrame::placeObjectButtonClicked() if (attached_bodies.empty()) { - RCLCPP_ERROR(LOGGER, "No bodies to place"); + RCLCPP_ERROR(logger_, "No bodies to place"); return; } @@ -357,7 +356,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 fb2d4afea0a..d9bc83f0876 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,7 +72,6 @@ QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subfra 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*/) { @@ -455,7 +454,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; @@ -520,7 +519,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(); }); @@ -539,7 +538,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(); }); @@ -560,7 +559,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(); }); @@ -584,7 +583,7 @@ void MotionPlanningFrame::computeDeleteSceneButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "%s", ex.what()); + RCLCPP_ERROR(logger_, "%s", ex.what()); } } else @@ -597,7 +596,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(); }); @@ -623,7 +622,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); }); } @@ -685,7 +684,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; @@ -695,17 +694,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()); @@ -724,7 +723,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()); } } @@ -753,7 +752,7 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "%s", ex.what()); + RCLCPP_ERROR(logger_, "%s", ex.what()); } if (got_q) @@ -780,7 +779,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()); } @@ -1050,10 +1049,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()); } } @@ -1065,7 +1064,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 eab7bfc3280..e79a0268064 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,7 +50,6 @@ namespace moveit_rviz_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_planning"); void MotionPlanningFrame::planButtonClicked() { @@ -100,7 +99,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(); @@ -114,7 +113,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); } @@ -129,7 +128,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))); @@ -146,7 +145,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 @@ -155,7 +154,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(); @@ -347,11 +346,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; } @@ -419,7 +418,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(); @@ -433,7 +432,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 faf3a538c50..245b7502696 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,7 +57,6 @@ 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 bf282d162d9..35d84a425e9 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,7 +46,6 @@ namespace moveit_rviz_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_frame_states"); void MotionPlanningFrame::populateRobotStatesList() { @@ -103,7 +102,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; @@ -158,7 +157,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 @@ -234,7 +233,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 3e37370448a..5fa7fe9019d 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,6 +221,7 @@ 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 23b16c06abd..ebb3edb4fed 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,14 +37,12 @@ #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 @@ -81,13 +79,14 @@ void BackgroundProcessing::processingThread() action_lock_.unlock(); try { - RCLCPP_DEBUG(LOGGER, "Begin executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(moveit::get_logger(), "Begin executing '%s'", action_name.c_str()); fn(); - RCLCPP_DEBUG(LOGGER, "Done executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(moveit::get_logger(), "Done executing '%s'", action_name.c_str()); } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Exception caught while processing action '%s': %s", action_name.c_str(), ex.what()); + RCLCPP_ERROR(moveit::get_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 75de575ec96..49d89dd6db0 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,15 +56,18 @@ #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) + : Display() + , planning_scene_needs_render_(true) + , current_scene_time_(0.0f) + , logger_(moveit::make_child_logger("planning_scene_display")) { move_group_ns_property_ = new rviz_common::properties::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in " @@ -273,7 +276,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(); } @@ -375,7 +378,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 48a16ad5981..2e11e94c61b 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,6 +42,7 @@ #include #include #include +#include #ifndef Q_MOC_RUN #include @@ -164,6 +165,7 @@ 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 1b828694359..73ad0223ebc 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,7 +62,6 @@ 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) @@ -73,6 +72,7 @@ TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Proper , widget_(widget) , trajectory_slider_panel_(nullptr) , trajectory_slider_dock_panel_(nullptr) + , logger_(moveit::make_child_logger("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 46f40828108..4d150234633 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,6 +95,7 @@ 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 701cd3e2942..2b96148e571 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -38,14 +38,15 @@ #include #include +#include #include namespace moveit_rviz_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.visualization.trajectory_display"); -TrajectoryDisplay::TrajectoryDisplay() : Display() +TrajectoryDisplay::TrajectoryDisplay() : Display(), logger_(moveit::make_child_logger("trajectory_display")) + { // The robot description property is only needed when using the trajectory playback standalone (not within motion // planning plugin) @@ -65,7 +66,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 7a5cd4aafa3..ac8ac874182 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(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 4370a106f91..6269e1dfb47 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(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 b1350890c4e..e3eebbebd75 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(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 187d2d5ab01..4ca6a754bcf 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(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 ffff2f9bdd5..c9d66f66c11 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(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 1136015636c..9d439deabfa 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::get_logger_mut() = node->get_logger(); + moveit::set_logger(node->get_logger()); std::string host;