diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index f0ef932dc75..de0be9ce667 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -167,8 +167,10 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi return false; if (cdata->req_->verbose) + { RCLCPP_DEBUG(getLogger(), "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); + } // see if we need to compute a contact std::size_t want_contact_count{ 0 }; @@ -534,8 +536,10 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void return false; } if (cdata->req->verbose) + { RCLCPP_DEBUG(getLogger(), "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); + } double dist_threshold = cdata->req->distance_threshold; diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 59b95ca2076..50b588e16bd 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -359,8 +359,10 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { if (!state.clearAttachedBody(aco.object.id)) + { RCLCPP_ERROR(getLogger(), "The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str()); + } } else RCLCPP_ERROR(getLogger(), "Unknown collision object operation: %d", aco.object.operation); diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h index 3c55c6b9279..9e4ff26f2b7 100644 --- a/moveit_core/utils/include/moveit/utils/moveit_error_code.h +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -79,7 +79,7 @@ class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes @param error_code Error code to be translated to a string @return Error code string */ -inline std::string errorCodeToString(MoveItErrorCode error_code) +inline std::string errorCodeToString(const MoveItErrorCode& error_code) { switch (error_code.val) { diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index c0c095f4932..1eaaf258cdb 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -58,9 +58,10 @@ #include static void publishOctomap(const rclcpp::Publisher::SharedPtr& octree_binary_pub, - occupancy_map_monitor::OccupancyMapMonitor& server, rclcpp::Logger logger) + occupancy_map_monitor::OccupancyMapMonitor& server) { octomap_msgs::msg::Octomap map; + const auto logger = moveit::getLogger("occupancy_map_monitor"); map.header.frame_id = server.getMapFrame(); map.header.stamp = rclcpp::Clock().now(); @@ -100,9 +101,8 @@ int main(int argc, char** argv) std::shared_ptr buffer = std::make_shared(clock_ptr, tf2::durationFromSec(5.0)); tf2_ros::TransformListener listener(*buffer, node, false /* spin_thread - disables executor */); occupancy_map_monitor::OccupancyMapMonitor server(node, buffer); - server.setUpdateCallback([&octree_binary_pub, &server, logger = node->get_logger()] { - return publishOctomap(octree_binary_pub, server, logger); - }); + server.setUpdateCallback( + [&octree_binary_pub, &server, logger = node->get_logger()] { return publishOctomap(octree_binary_pub, server); }); server.startMonitor(); rclcpp::spin(node); diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index b4158fb054c..23d6bd89f87 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -44,10 +44,9 @@ namespace moveit namespace planning_pipeline_interfaces { -rclcpp::Logger get_logger() +rclcpp::Logger getLogger() { - static rclcpp::Logger logger = moveit::getLogger("planning_pipeline_interfaces"); - return logger; + return moveit::getLogger("planning_pipeline_interfaces"); } ::planning_interface::MotionPlanResponse @@ -59,7 +58,7 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla auto it = planning_pipelines.find(motion_plan_request.pipeline_id); if (it == planning_pipelines.end()) { - RCLCPP_ERROR(get_logger(), "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); + RCLCPP_ERROR(getLogger(), "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; return motion_plan_response; } @@ -88,7 +87,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe const auto hardware_concurrency = std::thread::hardware_concurrency(); if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0) { - RCLCPP_WARN(get_logger(), + RCLCPP_WARN(getLogger(), "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " "hardware ('%d')", motion_plan_requests.size(), hardware_concurrency); @@ -106,7 +105,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::exception& e) { - RCLCPP_ERROR(get_logger(), "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); + RCLCPP_ERROR(getLogger(), "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); plan_solution = ::planning_interface::MotionPlanResponse(); plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; } @@ -118,7 +117,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe if (stopping_criterion_callback(plan_responses_container, motion_plan_requests)) { // Terminate planning pipelines - RCLCPP_INFO(get_logger(), "Stopping criterion met: Terminating planning pipelines that are still active"); + RCLCPP_INFO(getLogger(), "Stopping criterion met: Terminating planning pipelines that are still active"); for (const auto& request : motion_plan_requests) { try @@ -131,7 +130,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::out_of_range&) { - RCLCPP_WARN(get_logger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists", + RCLCPP_WARN(getLogger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists", request.pipeline_id.c_str()); } } @@ -174,7 +173,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, // Check if pipeline already exists if (planning_pipelines.count(planning_pipeline_name) > 0) { - RCLCPP_WARN(get_logger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_WARN(getLogger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } @@ -184,7 +183,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, if (!pipeline->getPlannerManager()) { - RCLCPP_ERROR(get_logger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 25aa2c9c5b4..652569658a9 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -1341,7 +1341,9 @@ MoveGroupInterface::~MoveGroupInterface() } MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) noexcept - : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_), logger_(other.logger_) + : remembered_joint_values_(std::move(other.remembered_joint_values_)) + , impl_(other.impl_) + , logger_(std::move(other.logger_)) { other.impl_ = nullptr; } diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index ae69f03ff6f..72592f9e7c9 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -57,7 +57,7 @@ typedef std::pair Lin typedef std::map LinkConstraintMap; void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap, - rclcpp::Logger logger) + const rclcpp::Logger& logger) { for (const moveit_msgs::msg::PositionConstraint& position_constraint : constraints.position_constraints) {