diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index d42ae85996..9491b463ea 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -52,12 +52,11 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.save_to_text"); - typedef std::pair LinkConstraintPair; typedef std::map LinkConstraintMap; -void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap) +void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap, + const rclcpp::Logger& logger) { for (const moveit_msgs::msg::PositionConstraint& position_constraint : constraints.position_constraints) { @@ -75,7 +74,7 @@ void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, Li } else { - RCLCPP_WARN(LOGGER, "Orientation constraint for %s has no matching position constraint", + RCLCPP_WARN(logger, "Orientation constraint for %s has no matching position constraint", orientation_constraint.link_name.c_str()); } } @@ -88,6 +87,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); + const auto logger = node->get_logger(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -125,7 +125,7 @@ int main(int argc, char** argv) moveit_warehouse::PlanningSceneWithMetadata pswm; if (pss.getPlanningScene(pswm, scene_name)) { - RCLCPP_INFO(LOGGER, "Saving scene '%s'", scene_name.c_str()); + RCLCPP_INFO(logger, "Saving scene '%s'", scene_name.c_str()); psm.getPlanningScene()->setPlanningSceneMsg(static_cast(*pswm)); std::ofstream fout((scene_name + ".scene").c_str()); psm.getPlanningScene()->saveGeometryToStream(fout); @@ -155,7 +155,7 @@ int main(int argc, char** argv) qfout << robot_state_names.size() << '\n'; for (const std::string& robot_state_name : robot_state_names) { - RCLCPP_INFO(LOGGER, "Saving start state %s for scene %s", robot_state_name.c_str(), scene_name.c_str()); + RCLCPP_INFO(logger, "Saving start state %s for scene %s", robot_state_name.c_str(), scene_name.c_str()); qfout << robot_state_name << '\n'; moveit_warehouse::RobotStateWithMetadata robot_state; rss.getRobotState(robot_state, robot_state_name); @@ -172,14 +172,14 @@ int main(int argc, char** argv) qfout << constraint_names.size() << '\n'; for (const std::string& constraint_name : constraint_names) { - RCLCPP_INFO(LOGGER, "Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str()); + RCLCPP_INFO(logger, "Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str()); qfout << "link_constraint" << '\n'; qfout << constraint_name << '\n'; moveit_warehouse::ConstraintsWithMetadata constraints; cs.getConstraints(constraints, constraint_name); LinkConstraintMap lcmap; - collectLinkConstraints(*constraints, lcmap); + collectLinkConstraints(*constraints, lcmap, logger); for (std::pair& iter : lcmap) { std::string link_name = iter.first; @@ -198,7 +198,7 @@ int main(int argc, char** argv) } } - RCLCPP_INFO(LOGGER, "Done."); + RCLCPP_INFO(logger, "Done."); rclcpp::spin(node); return 0; }