From 2ea4c0770ae3cc3dad4f0040da241db27df0ea76 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 12 Oct 2023 09:24:17 -0600 Subject: [PATCH 1/9] Node logger through singleton --- moveit_core/utils/CMakeLists.txt | 4 +- .../utils/include/moveit/utils/logger.hpp | 51 +++++++++++++++++ moveit_core/utils/src/logger.cpp | 57 +++++++++++++++++++ moveit_ros/warehouse/src/broadcast.cpp | 16 +++--- .../warehouse/src/constraints_storage.cpp | 10 ++-- moveit_ros/warehouse/src/import_from_text.cpp | 18 +++--- .../warehouse/src/initialize_demo_db.cpp | 14 +++-- .../warehouse/src/planning_scene_storage.cpp | 28 ++++----- .../src/planning_scene_world_storage.cpp | 10 ++-- moveit_ros/warehouse/src/save_as_text.cpp | 17 +++--- .../warehouse/src/save_to_warehouse.cpp | 51 +++++++++-------- moveit_ros/warehouse/src/state_storage.cpp | 10 ++-- .../src/trajectory_constraints_storage.cpp | 10 ++-- .../warehouse/src/warehouse_connector.cpp | 11 ++-- .../warehouse/src/warehouse_services.cpp | 29 +++++----- 15 files changed, 231 insertions(+), 105 deletions(-) create mode 100644 moveit_core/utils/include/moveit/utils/logger.hpp create mode 100644 moveit_core/utils/src/logger.cpp diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 7be8dd822d..5fa121ad7d 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -2,12 +2,13 @@ add_library(moveit_utils SHARED src/lexical_casts.cpp src/message_checks.cpp src/rclcpp_utils.cpp + src/logger.cpp ) target_include_directories(moveit_utils PUBLIC $ $ ) -ament_target_dependencies(moveit_utils Boost moveit_msgs) +ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp) set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -28,5 +29,6 @@ ament_target_dependencies(moveit_test_utils srdfdom urdfdom urdfdom_headers + rclcpp ) set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp new file mode 100644 index 0000000000..72ac08a8cb --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -0,0 +1,51 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include + +namespace moveit +{ + +// Function for getting a reference to a logger object kept on the stack +// Use get_logger_mut to set the logger to a node logger +const rclcpp::Logger& get_logger(); + +// Mutable access to global logger for setting to node logger +rclcpp::Logger& get_logger_mut(); + +} // namespace moveit diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp new file mode 100644 index 0000000000..0dd2d3ee6e --- /dev/null +++ b/moveit_core/utils/src/logger.cpp @@ -0,0 +1,57 @@ +/********************************************************************* + * 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 + +namespace moveit +{ + +// Function for getting a reference to a logger object kept on the stack +// Use get_logger_mut to set the logger to a node logger +const rclcpp::Logger& get_logger() +{ + return get_logger_mut(); +} + +// Mutable access to global logger for setting to node logger +rclcpp::Logger& get_logger_mut() +{ + static rclcpp::Logger logger = rclcpp::get_logger("moveit"); + return logger; +} + +} // namespace moveit diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index 208713d589..7a5cd4aafa 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/src/broadcast.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,7 @@ static const std::string CONSTRAINTS_TOPIC = "constraints"; static const std::string STATES_TOPIC = "robot_states"; using namespace std::chrono_literals; +using moveit::get_logger; int main(int argc, char** argv) { @@ -73,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); - const auto logger = node->get_logger(); + moveit::get_logger_mut() = node->get_logger(); // time to wait in between publishing messages double delay = 0.001; @@ -140,7 +142,7 @@ int main(int argc, char** argv) moveit_warehouse::PlanningSceneWithMetadata pswm; if (pss.getPlanningScene(pswm, scene_name)) { - RCLCPP_INFO(logger, "Publishing scene '%s'", + RCLCPP_INFO(get_logger(), "Publishing scene '%s'", pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str()); pub_scene->publish(static_cast(*pswm)); executor.spin_once(0ns); @@ -153,14 +155,14 @@ int main(int argc, char** argv) std::vector planning_queries; std::vector query_names; pss.getPlanningQueries(planning_queries, query_names, pswm->name); - RCLCPP_INFO(logger, "There are %d planning queries associated to the scene", + RCLCPP_INFO(get_logger(), "There are %d planning queries associated to the scene", static_cast(planning_queries.size())); rclcpp::sleep_for(500ms); for (std::size_t i = 0; i < planning_queries.size(); ++i) { if (req) { - RCLCPP_INFO(logger, "Publishing query '%s'", query_names[i].c_str()); + RCLCPP_INFO(get_logger(), "Publishing query '%s'", query_names[i].c_str()); pub_req->publish(static_cast(*planning_queries[i])); executor.spin_once(0ns); } @@ -194,7 +196,7 @@ int main(int argc, char** argv) moveit_warehouse::ConstraintsWithMetadata cwm; if (cs.getConstraints(cwm, cname)) { - RCLCPP_INFO(logger, "Publishing constraints '%s'", + RCLCPP_INFO(get_logger(), "Publishing constraints '%s'", cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str()); pub_constr->publish(static_cast(*cwm)); executor.spin_once(0ns); @@ -216,7 +218,7 @@ int main(int argc, char** argv) moveit_warehouse::RobotStateWithMetadata rswm; if (rs.getRobotState(rswm, rname)) { - RCLCPP_INFO(logger, "Publishing state '%s'", + RCLCPP_INFO(get_logger(), "Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str()); pub_state->publish(static_cast(*rswm)); executor.spin_once(0ns); @@ -226,7 +228,7 @@ int main(int argc, char** argv) } rclcpp::sleep_for(1s); - RCLCPP_INFO(logger, "Done."); + RCLCPP_INFO(get_logger(), "Done."); return 0; } diff --git a/moveit_ros/warehouse/src/constraints_storage.cpp b/moveit_ros/warehouse/src/constraints_storage.cpp index f725859414..ccd9e5a15f 100644 --- a/moveit_ros/warehouse/src/constraints_storage.cpp +++ b/moveit_ros/warehouse/src/constraints_storage.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include @@ -44,8 +45,7 @@ const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME = "c const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id"; const std::string moveit_warehouse::ConstraintsStorage::ROBOT_NAME = "robot_id"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.constraints_storage"); - +using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; @@ -81,7 +81,7 @@ void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::msg metadata->append(ROBOT_NAME, robot); metadata->append(CONSTRAINTS_GROUP_NAME, group); constraints_collection_->insert(msg, metadata); - RCLCPP_DEBUG(LOGGER, "%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str()); + RCLCPP_DEBUG(get_logger(), "%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str()); } bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string& name, const std::string& robot, @@ -158,7 +158,7 @@ void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string& Metadata::Ptr m = constraints_collection_->createMetadata(); m->append(CONSTRAINTS_ID_NAME, new_name); constraints_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(LOGGER, "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& name, const std::string& robot, @@ -171,5 +171,5 @@ void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& if (!group.empty()) q->append(CONSTRAINTS_GROUP_NAME, group); unsigned int rem = constraints_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u Constraints messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u Constraints messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index 6b91501433..bded956345 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -45,10 +45,11 @@ #include #include #include +#include -static const std::string ROBOT_DESCRIPTION = "robot_description"; +using moveit::get_logger; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.planning_scene_storage"); +static const std::string ROBOT_DESCRIPTION = "robot_description"; void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* psm, moveit_warehouse::RobotStateStorage* rs) @@ -90,7 +91,7 @@ void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* st.setVariablePositions(v); moveit_msgs::msg::RobotState msg; moveit::core::robotStateToRobotStateMsg(st, msg); - RCLCPP_INFO(LOGGER, "Parsed start state '%s'", name.c_str()); + RCLCPP_INFO(get_logger(), "Parsed start state '%s'", name.c_str()); rs->addRobotState(msg, name); } } @@ -135,7 +136,7 @@ void parseLinkConstraint(std::istream& in, planning_scene_monitor::PlanningScene Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())); } else - RCLCPP_ERROR(LOGGER, "Unknown link constraint element: '%s'", type.c_str()); + RCLCPP_ERROR(get_logger(), "Unknown link constraint element: '%s'", type.c_str()); in >> type; } @@ -150,7 +151,7 @@ void parseLinkConstraint(std::istream& in, planning_scene_monitor::PlanningScene pose.header.frame_id = psm->getRobotModel()->getModelFrame(); moveit_msgs::msg::Constraints constr = kinematic_constraints::constructGoalConstraints(link_name, pose); constr.name = name; - RCLCPP_INFO(LOGGER, "Parsed link constraint '%s'", name.c_str()); + RCLCPP_INFO(get_logger(), "Parsed link constraint '%s'", name.c_str()); cs->addConstraints(constr); } } @@ -180,7 +181,7 @@ void parseGoal(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* p } else { - RCLCPP_INFO(LOGGER, "Unknown goal type: '%s'", type.c_str()); + RCLCPP_INFO(get_logger(), "Unknown goal type: '%s'", type.c_str()); } } } @@ -209,7 +210,7 @@ void parseQueries(std::istream& in, planning_scene_monitor::PlanningSceneMonitor } else { - RCLCPP_ERROR(LOGGER, "Unknown query type: '%s'", type.c_str()); + RCLCPP_ERROR(get_logger(), "Unknown query type: '%s'", type.c_str()); } } } @@ -222,6 +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(); // clang-format off boost::program_options::options_description desc; @@ -251,7 +253,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(LOGGER, "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor"); return 1; } diff --git a/moveit_ros/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/src/initialize_demo_db.cpp index 3123e48428..b1350890c4 100644 --- a/moveit_ros/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/src/initialize_demo_db.cpp @@ -52,10 +52,11 @@ #include #include #include +#include static const std::string ROBOT_DESCRIPTION = "robot_description"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.initialize_demo_db"); +using moveit::get_logger; int main(int argc, char** argv) { @@ -64,6 +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(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -90,7 +92,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(LOGGER, "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor"); return 1; } @@ -106,12 +108,12 @@ int main(int argc, char** argv) psm.getPlanningScene()->getPlanningSceneMsg(psmsg); psmsg.name = "default"; pss.addPlanningScene(psmsg); - RCLCPP_INFO(LOGGER, "Added default scene: '%s'", psmsg.name.c_str()); + RCLCPP_INFO(get_logger(), "Added default scene: '%s'", psmsg.name.c_str()); moveit_msgs::msg::RobotState rsmsg; moveit::core::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg); rs.addRobotState(rsmsg, "default"); - RCLCPP_INFO(LOGGER, "Added default state"); + RCLCPP_INFO(get_logger(), "Added default state"); const std::vector& gnames = psm.getRobotModel()->getJointModelGroupNames(); for (const std::string& gname : gnames) @@ -138,9 +140,9 @@ int main(int argc, char** argv) cmsg.orientation_constraints.resize(1, ocm); cmsg.name = ocm.link_name + ":upright"; cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName()); - RCLCPP_INFO(LOGGER, "Added default constraint: '%s'", cmsg.name.c_str()); + RCLCPP_INFO(get_logger(), "Added default constraint: '%s'", cmsg.name.c_str()); } - RCLCPP_INFO(LOGGER, "Default MoveIt Warehouse was reset."); + RCLCPP_INFO(get_logger(), "Default MoveIt Warehouse was reset."); rclcpp::spin(node); diff --git a/moveit_ros/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/src/planning_scene_storage.cpp index ae7b9710ea..b3aec0b8ad 100644 --- a/moveit_ros/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp @@ -38,17 +38,17 @@ #include #include #include +#include const std::string moveit_warehouse::PlanningSceneStorage::DATABASE_NAME = "moveit_planning_scenes"; const std::string moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME = "planning_scene_id"; const std::string moveit_warehouse::PlanningSceneStorage::MOTION_PLAN_REQUEST_ID_NAME = "motion_request_id"; +using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.planning_scene_storage"); - moveit_warehouse::PlanningSceneStorage::PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn) : MoveItMessageStorage(std::move(conn)) { @@ -85,7 +85,7 @@ void moveit_warehouse::PlanningSceneStorage::addPlanningScene(const moveit_msgs: Metadata::Ptr metadata = planning_scene_collection_->createMetadata(); metadata->append(PLANNING_SCENE_ID_NAME, scene.name); planning_scene_collection_->insert(scene, metadata); - RCLCPP_DEBUG(LOGGER, "%s scene '%s'", replace ? "Replaced" : "Added", scene.name.c_str()); + RCLCPP_DEBUG(get_logger(), "%s scene '%s'", replace ? "Replaced" : "Added", scene.name.c_str()); } bool moveit_warehouse::PlanningSceneStorage::hasPlanningScene(const std::string& name) const @@ -173,7 +173,7 @@ moveit_warehouse::PlanningSceneStorage::addNewPlanningRequest(const moveit_msgs: metadata->append(PLANNING_SCENE_ID_NAME, scene_name); metadata->append(MOTION_PLAN_REQUEST_ID_NAME, id); motion_plan_request_collection_->insert(planning_query, metadata); - RCLCPP_DEBUG(LOGGER, "Saved planning query '%s' for scene '%s'", id.c_str(), scene_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Saved planning query '%s' for scene '%s'", id.c_str(), scene_name.c_str()); return id; } @@ -231,7 +231,7 @@ bool moveit_warehouse::PlanningSceneStorage::getPlanningScene(PlanningSceneWithM std::vector planning_scenes = planning_scene_collection_->queryList(q, false); if (planning_scenes.empty()) { - RCLCPP_WARN(LOGGER, "Planning scene '%s' was not found in the database", scene_name.c_str()); + RCLCPP_WARN(get_logger(), "Planning scene '%s' was not found in the database", scene_name.c_str()); return false; } scene_m = planning_scenes.back(); @@ -251,7 +251,7 @@ bool moveit_warehouse::PlanningSceneStorage::getPlanningQuery(MotionPlanRequestW std::vector planning_queries = motion_plan_request_collection_->queryList(q, false); if (planning_queries.empty()) { - RCLCPP_ERROR(LOGGER, "Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str()); + RCLCPP_ERROR(get_logger(), "Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str()); return false; } else @@ -369,7 +369,7 @@ void moveit_warehouse::PlanningSceneStorage::renamePlanningScene(const std::stri Metadata::Ptr m = planning_scene_collection_->createMetadata(); m->append(PLANNING_SCENE_ID_NAME, new_scene_name); planning_scene_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(LOGGER, "Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::renamePlanningQuery(const std::string& scene_name, @@ -382,7 +382,7 @@ void moveit_warehouse::PlanningSceneStorage::renamePlanningQuery(const std::stri Metadata::Ptr m = motion_plan_request_collection_->createMetadata(); m->append(MOTION_PLAN_REQUEST_ID_NAME, new_query_name); motion_plan_request_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(LOGGER, "Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), + RCLCPP_DEBUG(get_logger(), "Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), old_query_name.c_str(), new_query_name.c_str()); } @@ -392,7 +392,7 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningScene(const std::stri Query::Ptr q = planning_scene_collection_->createQuery(); q->append(PLANNING_SCENE_ID_NAME, scene_name); unsigned int rem = planning_scene_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningQueries(const std::string& scene_name) @@ -401,7 +401,7 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningQueries(const std::st Query::Ptr q = motion_plan_request_collection_->createQuery(); q->append(PLANNING_SCENE_ID_NAME, scene_name); unsigned int rem = motion_plan_request_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningQuery(const std::string& scene_name, @@ -412,8 +412,8 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningQuery(const std::stri q->append(PLANNING_SCENE_ID_NAME, scene_name); q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name); unsigned int rem = motion_plan_request_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, scene_name.c_str(), - query_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, + scene_name.c_str(), query_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string& scene_name) @@ -421,7 +421,7 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::st Query::Ptr q = robot_trajectory_collection_->createQuery(); q->append(PLANNING_SCENE_ID_NAME, scene_name); unsigned int rem = robot_trajectory_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string& scene_name, @@ -431,6 +431,6 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::st q->append(PLANNING_SCENE_ID_NAME, scene_name); q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name); unsigned int rem = robot_trajectory_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(), + RCLCPP_DEBUG(get_logger(), "Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(), query_name.c_str()); } diff --git a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp index 310e060f56..9934bbd5bd 100644 --- a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp @@ -35,14 +35,14 @@ /* Author: Ioan Sucan */ #include +#include #include const std::string moveit_warehouse::PlanningSceneWorldStorage::DATABASE_NAME = "moveit_planning_scene_worlds"; const std::string moveit_warehouse::PlanningSceneWorldStorage::PLANNING_SCENE_WORLD_ID_NAME = "world_id"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.planning_scene_world_storage"); - +using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; @@ -77,7 +77,7 @@ void moveit_warehouse::PlanningSceneWorldStorage::addPlanningSceneWorld(const mo Metadata::Ptr metadata = planning_scene_world_collection_->createMetadata(); metadata->append(PLANNING_SCENE_WORLD_ID_NAME, name); planning_scene_world_collection_->insert(msg, metadata); - RCLCPP_DEBUG(LOGGER, "%s planning scene world '%s'", replace ? "Replaced" : "Added", name.c_str()); + RCLCPP_DEBUG(get_logger(), "%s planning scene world '%s'", replace ? "Replaced" : "Added", name.c_str()); } bool moveit_warehouse::PlanningSceneWorldStorage::hasPlanningSceneWorld(const std::string& name) const @@ -133,7 +133,7 @@ void moveit_warehouse::PlanningSceneWorldStorage::renamePlanningSceneWorld(const Metadata::Ptr m = planning_scene_world_collection_->createMetadata(); m->append(PLANNING_SCENE_WORLD_ID_NAME, new_name); planning_scene_world_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(LOGGER, "Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::PlanningSceneWorldStorage::removePlanningSceneWorld(const std::string& name) @@ -141,5 +141,5 @@ void moveit_warehouse::PlanningSceneWorldStorage::removePlanningSceneWorld(const Query::Ptr q = planning_scene_world_collection_->createQuery(); q->append(PLANNING_SCENE_WORLD_ID_NAME, name); unsigned int rem = planning_scene_world_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u PlanningSceneWorld messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u PlanningSceneWorld messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index d42ae85996..187d2d5ab0 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -49,14 +49,15 @@ #include #include #include +#include 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; +using moveit::get_logger; + void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap) { for (const moveit_msgs::msg::PositionConstraint& position_constraint : constraints.position_constraints) @@ -75,7 +76,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(get_logger(), "Orientation constraint for %s has no matching position constraint", orientation_constraint.link_name.c_str()); } } @@ -88,6 +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(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -125,7 +127,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(get_logger(), "Saving scene '%s'", scene_name.c_str()); psm.getPlanningScene()->setPlanningSceneMsg(static_cast(*pswm)); std::ofstream fout((scene_name + ".scene").c_str()); psm.getPlanningScene()->saveGeometryToStream(fout); @@ -155,7 +157,8 @@ int main(int argc, char** argv) qfout << robot_state_names.size() << '\n'; for (const std::string& robot_state_name : robot_state_names) { - RCLCPP_INFO(LOGGER, "Saving start state %s for scene %s", robot_state_name.c_str(), scene_name.c_str()); + RCLCPP_INFO(get_logger(), "Saving start state %s for scene %s", robot_state_name.c_str(), + scene_name.c_str()); qfout << robot_state_name << '\n'; moveit_warehouse::RobotStateWithMetadata robot_state; rss.getRobotState(robot_state, robot_state_name); @@ -172,7 +175,7 @@ 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(get_logger(), "Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str()); qfout << "link_constraint" << '\n'; qfout << constraint_name << '\n'; moveit_warehouse::ConstraintsWithMetadata constraints; @@ -198,7 +201,7 @@ int main(int argc, char** argv) } } - RCLCPP_INFO(LOGGER, "Done."); + RCLCPP_INFO(get_logger(), "Done."); rclcpp::spin(node); return 0; } diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index 894212587d..ffff2f9bdd 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -59,13 +60,13 @@ #include #include -static const std::string ROBOT_DESCRIPTION = "robot_description"; +using moveit::get_logger; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.save_to_warehouse"); +static const std::string ROBOT_DESCRIPTION = "robot_description"; void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor& psm, moveit_warehouse::PlanningSceneStorage& pss) { - RCLCPP_INFO(LOGGER, "Received an update to the planning scene..."); + RCLCPP_INFO(get_logger(), "Received an update to the planning scene..."); if (!psm.getPlanningScene()->getName().empty()) { @@ -77,12 +78,12 @@ void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor& psm, moveit_war } else { - RCLCPP_INFO(LOGGER, "Scene '%s' was previously added. Not adding again.", + RCLCPP_INFO(get_logger(), "Scene '%s' was previously added. Not adding again.", psm.getPlanningScene()->getName().c_str()); } } else - RCLCPP_INFO(LOGGER, "Scene name is empty. Not saving."); + RCLCPP_INFO(get_logger(), "Scene name is empty. Not saving."); } void onMotionPlanRequest(const moveit_msgs::msg::MotionPlanRequest& req, @@ -90,7 +91,7 @@ void onMotionPlanRequest(const moveit_msgs::msg::MotionPlanRequest& req, { if (psm.getPlanningScene()->getName().empty()) { - RCLCPP_INFO(LOGGER, "Scene name is empty. Not saving planning request."); + RCLCPP_INFO(get_logger(), "Scene name is empty. Not saving planning request."); return; } pss.addPlanningQuery(req, psm.getPlanningScene()->getName()); @@ -100,19 +101,19 @@ void onConstraints(const moveit_msgs::msg::Constraints& msg, moveit_warehouse::C { if (msg.name.empty()) { - RCLCPP_INFO(LOGGER, "No name specified for constraints. Not saving."); + RCLCPP_INFO(get_logger(), "No name specified for constraints. Not saving."); return; } if (cs.hasConstraints(msg.name)) { - RCLCPP_INFO(LOGGER, "Replacing constraints '%s'", msg.name.c_str()); + RCLCPP_INFO(get_logger(), "Replacing constraints '%s'", msg.name.c_str()); cs.removeConstraints(msg.name); cs.addConstraints(msg); } else { - RCLCPP_INFO(LOGGER, "Adding constraints '%s'", msg.name.c_str()); + RCLCPP_INFO(get_logger(), "Adding constraints '%s'", msg.name.c_str()); cs.addConstraints(msg); } } @@ -126,7 +127,7 @@ void onRobotState(const moveit_msgs::msg::RobotState& msg, moveit_warehouse::Rob while (names_set.find("S" + std::to_string(n)) != names_set.end()) n++; std::string name = "S" + std::to_string(n); - RCLCPP_INFO(LOGGER, "Adding robot state '%s'", name.c_str()); + RCLCPP_INFO(get_logger(), "Adding robot state '%s'", name.c_str()); rs.addRobotState(msg, name); } @@ -137,6 +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(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -163,7 +165,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(LOGGER, "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor"); return 1; } @@ -176,35 +178,35 @@ int main(int argc, char** argv) pss.getPlanningSceneNames(names); if (names.empty()) { - RCLCPP_INFO(LOGGER, "There are no previously stored scenes"); + RCLCPP_INFO(get_logger(), "There are no previously stored scenes"); } else { - RCLCPP_INFO(LOGGER, "Previously stored scenes:"); + RCLCPP_INFO(get_logger(), "Previously stored scenes:"); for (const std::string& name : names) - RCLCPP_INFO(LOGGER, " * %s", name.c_str()); + RCLCPP_INFO(get_logger(), " * %s", name.c_str()); } cs.getKnownConstraints(names); if (names.empty()) { - RCLCPP_INFO(LOGGER, "There are no previously stored constraints"); + RCLCPP_INFO(get_logger(), "There are no previously stored constraints"); } else { - RCLCPP_INFO(LOGGER, "Previously stored constraints:"); + RCLCPP_INFO(get_logger(), "Previously stored constraints:"); for (const std::string& name : names) - RCLCPP_INFO(LOGGER, " * %s", name.c_str()); + RCLCPP_INFO(get_logger(), " * %s", name.c_str()); } rs.getKnownRobotStates(names); if (names.empty()) { - RCLCPP_INFO(LOGGER, "There are no previously stored robot states"); + RCLCPP_INFO(get_logger(), "There are no previously stored robot states"); } else { - RCLCPP_INFO(LOGGER, "Previously stored robot states:"); + RCLCPP_INFO(get_logger(), "Previously stored robot states:"); for (const std::string& name : names) - RCLCPP_INFO(LOGGER, " * %s", name.c_str()); + RCLCPP_INFO(get_logger(), " * %s", name.c_str()); } psm.addUpdateCallback([&](auto&&) { return onSceneUpdate(psm, pss); }); @@ -221,10 +223,11 @@ int main(int argc, char** argv) std::vector topics; psm.getMonitoredTopics(topics); - RCLCPP_INFO_STREAM(LOGGER, "Listening for scene updates on topics " << fmt::format("{}", fmt::join(topics, ", "))); - RCLCPP_INFO_STREAM(LOGGER, "Listening for planning requests on topic " << mplan_req_sub->get_topic_name()); - RCLCPP_INFO_STREAM(LOGGER, "Listening for named constraints on topic " << constr_sub->get_topic_name()); - RCLCPP_INFO_STREAM(LOGGER, "Listening for states on topic " << state_sub->get_topic_name()); + RCLCPP_INFO_STREAM(get_logger(), + "Listening for scene updates on topics " << fmt::format("{}", fmt::join(topics, ", "))); + RCLCPP_INFO_STREAM(get_logger(), "Listening for planning requests on topic " << mplan_req_sub->get_topic_name()); + RCLCPP_INFO_STREAM(get_logger(), "Listening for named constraints on topic " << constr_sub->get_topic_name()); + RCLCPP_INFO_STREAM(get_logger(), "Listening for states on topic " << state_sub->get_topic_name()); rclcpp::spin(node); return 0; diff --git a/moveit_ros/warehouse/src/state_storage.cpp b/moveit_ros/warehouse/src/state_storage.cpp index cea0e6a52f..d5c7ad205e 100644 --- a/moveit_ros/warehouse/src/state_storage.cpp +++ b/moveit_ros/warehouse/src/state_storage.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include @@ -43,8 +44,7 @@ const std::string moveit_warehouse::RobotStateStorage::DATABASE_NAME = "moveit_r const std::string moveit_warehouse::RobotStateStorage::STATE_NAME = "state_id"; const std::string moveit_warehouse::RobotStateStorage::ROBOT_NAME = "robot_id"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.state_storage"); - +using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; @@ -79,7 +79,7 @@ void moveit_warehouse::RobotStateStorage::addRobotState(const moveit_msgs::msg:: metadata->append(STATE_NAME, name); metadata->append(ROBOT_NAME, robot); state_collection_->insert(msg, metadata); - RCLCPP_DEBUG(LOGGER, "%s robot state '%s'", replace ? "Replaced" : "Added", name.c_str()); + RCLCPP_DEBUG(get_logger(), "%s robot state '%s'", replace ? "Replaced" : "Added", name.c_str()); } bool moveit_warehouse::RobotStateStorage::hasRobotState(const std::string& name, const std::string& robot) const @@ -143,7 +143,7 @@ void moveit_warehouse::RobotStateStorage::renameRobotState(const std::string& ol Metadata::Ptr m = state_collection_->createMetadata(); m->append(STATE_NAME, new_name); state_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(LOGGER, "Renamed robot state from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Renamed robot state from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::RobotStateStorage::removeRobotState(const std::string& name, const std::string& robot) @@ -153,5 +153,5 @@ void moveit_warehouse::RobotStateStorage::removeRobotState(const std::string& na if (!robot.empty()) q->append(ROBOT_NAME, robot); unsigned int rem = state_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u RobotState messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u RobotState messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp index 8ba11a3ba1..fcc8744db0 100644 --- a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp @@ -35,6 +35,7 @@ /* Author: Mario Prats, Ioan Sucan */ #include +#include #include @@ -44,8 +45,7 @@ const std::string moveit_warehouse::TrajectoryConstraintsStorage::CONSTRAINTS_ID const std::string moveit_warehouse::TrajectoryConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id"; const std::string moveit_warehouse::TrajectoryConstraintsStorage::ROBOT_NAME = "robot_id"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.trajectory_constraints_storage"); - +using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; @@ -83,7 +83,7 @@ void moveit_warehouse::TrajectoryConstraintsStorage::addTrajectoryConstraints( metadata->append(ROBOT_NAME, robot); metadata->append(CONSTRAINTS_GROUP_NAME, group); constraints_collection_->insert(msg, metadata); - RCLCPP_DEBUG(LOGGER, "%s constraints '%s'", replace ? "Replaced" : "Added", name.c_str()); + RCLCPP_DEBUG(get_logger(), "%s constraints '%s'", replace ? "Replaced" : "Added", name.c_str()); } bool moveit_warehouse::TrajectoryConstraintsStorage::hasTrajectoryConstraints(const std::string& name, @@ -165,7 +165,7 @@ void moveit_warehouse::TrajectoryConstraintsStorage::renameTrajectoryConstraints Metadata::Ptr m = constraints_collection_->createMetadata(); m->append(CONSTRAINTS_ID_NAME, new_name); constraints_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(LOGGER, "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::TrajectoryConstraintsStorage::removeTrajectoryConstraints(const std::string& name, @@ -179,5 +179,5 @@ void moveit_warehouse::TrajectoryConstraintsStorage::removeTrajectoryConstraints if (!group.empty()) q->append(CONSTRAINTS_GROUP_NAME, group); unsigned int rem = constraints_collection_->removeMessages(q); - RCLCPP_DEBUG(LOGGER, "Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(get_logger(), "Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/warehouse_connector.cpp b/moveit_ros/warehouse/src/warehouse_connector.cpp index f75123c50b..b87590b16e 100644 --- a/moveit_ros/warehouse/src/warehouse_connector.cpp +++ b/moveit_ros/warehouse/src/warehouse_connector.cpp @@ -40,17 +40,18 @@ #include #include #include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.warehouse_connector"); +using moveit::get_logger; #ifdef _WIN32 void kill(int, int) { - RCLCPP_ERROR(LOGGER, "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(get_logger(), "Warehouse connector not supported on Windows"); } // Should never be called int fork() { - RCLCPP_ERROR(LOGGER, "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(get_logger(), "Warehouse connector not supported on Windows"); return -1; } #else @@ -77,7 +78,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname) child_pid_ = fork(); if (child_pid_ == -1) { - RCLCPP_ERROR(LOGGER, "Error forking process."); + RCLCPP_ERROR(get_logger(), "Error forking process."); child_pid_ = 0; return false; } @@ -105,7 +106,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname) delete[] argv[1]; delete[] argv[2]; delete[] argv; - RCLCPP_ERROR_STREAM(LOGGER, + RCLCPP_ERROR_STREAM(get_logger(), "execv() returned " << code << ", errno=" << errno << " string errno = " << strerror(errno)); } return false; diff --git a/moveit_ros/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/src/warehouse_services.cpp index 46c7e8046c..1136015636 100644 --- a/moveit_ros/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/src/warehouse_services.cpp @@ -48,10 +48,11 @@ #include #include #include +#include -static const std::string ROBOT_DESCRIPTION = "robot_description"; +using moveit::get_logger; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.warehouse_services"); +static const std::string ROBOT_DESCRIPTION = "robot_description"; bool storeState(const std::shared_ptr& request, const std::shared_ptr& response, @@ -59,7 +60,7 @@ bool storeState(const std::shared_ptrname.empty()) { - RCLCPP_ERROR(LOGGER, "You must specify a name to store a state"); + RCLCPP_ERROR(get_logger(), "You must specify a name to store a state"); return (response->success = false); } rs.addRobotState(request->state, request->name, request->robot); @@ -95,7 +96,7 @@ bool getState(const std::shared_ptrname, request->robot)) { - RCLCPP_ERROR_STREAM(LOGGER, "No state called '" << request->name << "' for robot '" << request->robot << "'."); + RCLCPP_ERROR_STREAM(get_logger(), "No state called '" << request->name << "' for robot '" << request->robot << "'."); moveit_msgs::msg::RobotState dummy; response->state = dummy; return false; @@ -112,7 +113,8 @@ bool renameState(const std::shared_ptrold_name, request->robot)) { - RCLCPP_ERROR_STREAM(LOGGER, "No state called '" << request->old_name << "' for robot '" << request->robot << "'."); + RCLCPP_ERROR_STREAM(get_logger(), + "No state called '" << request->old_name << "' for robot '" << request->robot << "'."); return false; } rs.renameRobotState(request->old_name, request->new_name, request->robot); @@ -125,7 +127,7 @@ bool deleteState(const std::shared_ptrname, request->robot)) { - RCLCPP_ERROR_STREAM(LOGGER, "No state called '" << request->name << "' for robot '" << request->robot << "'."); + RCLCPP_ERROR_STREAM(get_logger(), "No state called '" << request->name << "' for robot '" << request->robot << "'."); return false; } rs.removeRobotState(request->name, request->robot); @@ -139,6 +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(); std::string host; @@ -158,23 +161,23 @@ int main(int argc, char** argv) conn = moveit_warehouse::loadDatabase(node); conn->setParams(host, port, connection_timeout); - RCLCPP_INFO(LOGGER, "Connecting to warehouse on %s:%d", host.c_str(), port); + RCLCPP_INFO(get_logger(), "Connecting to warehouse on %s:%d", host.c_str(), port); int tries = 0; while (!conn->connect()) { ++tries; - RCLCPP_WARN(LOGGER, "Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries, + RCLCPP_WARN(get_logger(), "Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries, connection_retries); if (tries == connection_retries) { - RCLCPP_FATAL(LOGGER, "Failed to connect too many times, giving up"); + RCLCPP_FATAL(get_logger(), "Failed to connect too many times, giving up"); return 1; } } } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "%s", ex.what()); + RCLCPP_ERROR(get_logger(), "%s", ex.what()); return 1; } @@ -184,13 +187,13 @@ int main(int argc, char** argv) rs.getKnownRobotStates(names); if (names.empty()) { - RCLCPP_INFO(LOGGER, "There are no previously stored robot states"); + RCLCPP_INFO(get_logger(), "There are no previously stored robot states"); } else { - RCLCPP_INFO(LOGGER, "Previously stored robot states:"); + RCLCPP_INFO(get_logger(), "Previously stored robot states:"); for (const std::string& name : names) - RCLCPP_INFO(LOGGER, " * %s", name.c_str()); + RCLCPP_INFO(get_logger(), " * %s", name.c_str()); } auto save_cb = [&](const std::shared_ptr& request, From 7bdbfcb37829c352f6d8bf88a19325007b48f22c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 13 Oct 2023 11:23:15 -0600 Subject: [PATCH 2/9] Update moveit_core/utils/include/moveit/utils/logger.hpp Co-authored-by: Abishalini Sivaraman --- moveit_core/utils/include/moveit/utils/logger.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 72ac08a8cb..1296504485 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include namespace moveit { From a02cbc66aeea0baf8e6187df2284fd215e781718 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 23 Oct 2023 15:21:34 -0600 Subject: [PATCH 3/9] tests --- moveit_core/package.xml | 3 + moveit_core/utils/CMakeLists.txt | 4 + .../utils/include/moveit/utils/logger.hpp | 6 + moveit_core/utils/src/logger.cpp | 7 + moveit_core/utils/test/CMakeLists.txt | 11 ++ moveit_core/utils/test/logger_launch_test.py | 122 ++++++++++++++++++ moveit_core/utils/test/logger_test_node.cpp | 72 +++++++++++ 7 files changed, 225 insertions(+) create mode 100644 moveit_core/utils/test/CMakeLists.txt create mode 100644 moveit_core/utils/test/logger_launch_test.py create mode 100644 moveit_core/utils/test/logger_test_node.cpp diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 1cbe79cfb7..85c2c2633a 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -72,6 +72,9 @@ ament_cmake_gtest ament_cmake_gmock ament_index_cpp + launch_testing_ament_cmake + rclpy + rcl_interfaces ament_lint_auto ament_lint_common diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 5fa121ad7d..a206662a19 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -32,3 +32,7 @@ ament_target_dependencies(moveit_test_utils rclcpp ) set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +if(BUILD_TESTING) + add_subdirectory(test) +endif() diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 1296504485..b61f1252c5 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -37,6 +37,7 @@ #pragma once #include +#include namespace moveit { @@ -45,6 +46,11 @@ namespace moveit // Use get_logger_mut 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 +// Do no use this in place as it will create a new logger each time +// instead store it in the state of your class or method. +rclcpp::Logger get_child_logger(const std::string& name); + // Mutable access to global logger for setting to node logger rclcpp::Logger& get_logger_mut(); diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index 0dd2d3ee6e..8f5a593fa6 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -36,6 +36,7 @@ #include #include +#include namespace moveit { @@ -47,6 +48,12 @@ const rclcpp::Logger& get_logger() return get_logger_mut(); } +rclcpp::Logger get_child_logger(const std::string& name) +{ + auto logger = get_logger_mut().get_child(name); + return logger; +} + // Mutable access to global logger for setting to node logger rclcpp::Logger& get_logger_mut() { diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt new file mode 100644 index 0000000000..c4a5ca29fd --- /dev/null +++ b/moveit_core/utils/test/CMakeLists.txt @@ -0,0 +1,11 @@ + +find_package(launch_testing_ament_cmake) + +add_executable(logger_test_node logger_test_node.cpp) +target_link_libraries(logger_test_node rclcpp::rclcpp moveit_utils) +install(TARGETS logger_test_node + DESTINATION lib/${PROJECT_NAME}) + + +# Runs logget_test_node and observes ros logging +add_launch_test(logger_launch_test.py) diff --git a/moveit_core/utils/test/logger_launch_test.py b/moveit_core/utils/test/logger_launch_test.py new file mode 100644 index 0000000000..0817470d90 --- /dev/null +++ b/moveit_core/utils/test/logger_launch_test.py @@ -0,0 +1,122 @@ +# 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 + +# Based on https://github.com/ros2/launch/blob/master/launch_testing/test/launch_testing/examples/good_proc_launch_test.py + +import os +import unittest +from threading import Event +from threading import Thread + +import launch +import launch_ros +import launch_testing +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import Log + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + # dut: device under test + dut_process = launch_ros.actions.Node( + package="moveit_core", + executable="logger_test_node", + name="logger_test_node", + output="screen", + ) + + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + return launch.LaunchDescription( + [ + dut_process, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ), {"dut_process": dut_process} + + +class MakeTestNode(Node): + def __init__(self, name="test_node"): + super().__init__(name) + self.msg_event_object = Event() + self.rosout_msgs = [] + + def start_subscriber(self): + # Create a subscriber + self.subscription = self.create_subscription( + Log, "rosout", self.subscriber_callback, 10 + ) + + # Add a spin thread + self.ros_spin_thread = Thread( + target=lambda node: rclpy.spin(node), args=(self,) + ) + self.ros_spin_thread.start() + + def subscriber_callback(self, data): + self.rosout_msgs.append(data) + self.msg_event_object.set() + + +# These tests will run concurrently with the dut process. After all these tests are done, +# the launch system will shut down the processes that it started up +class TestFixture(unittest.TestCase): + def test_stdout_print(self, proc_output): + proc_output.assertWaitFor("Before", timeout=10, stream="stdout") + proc_output.assertWaitFor("After", timeout=10, stream="stdout") + proc_output.assertWaitFor("Child", timeout=10, stream="stdout") + + def test_rosout_msgs_published(self, proc_output): + rclpy.init() + try: + node = MakeTestNode("test_node") + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=5.0) + assert msgs_received_flag, "Did not receive msgs !" + finally: + rclpy.shutdown() + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/moveit_core/utils/test/logger_test_node.cpp b/moveit_core/utils/test/logger_test_node.cpp new file mode 100644 index 0000000000..a73635e236 --- /dev/null +++ b/moveit_core/utils/test/logger_test_node.cpp @@ -0,0 +1,72 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Robotics Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Tyler Weaver */ + +#include +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("logger_test_node"); + + // Not a node logger, should print but should not be in file or rosout output + RCLCPP_INFO(moveit::get_logger(), "Before"); + + // Set the node logger + moveit::get_logger_mut() = node->get_logger(); + + // A node logger, should be in the file output and rosout + RCLCPP_INFO(moveit::get_logger(), "After"); + + // A child logger, should also be in the file output and rosout + const auto child_logger = moveit::get_child_logger("child"); + RCLCPP_INFO(child_logger, "Child"); + + // Spin the node to publish to /rosout +#if RCLCPP_VERSION_GTE(22, 1, 0) // https://github.com/ros2/rclcpp/commit/a17d26b20ac41cc9d5bf3583de8475bb7c18bb1e + rclcpp::spin_all(node, std::chrono::seconds(1)); +#else // Humble or Iron + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node); + exec.spin_all(std::chrono::seconds(1)); +#endif + + std::this_thread::sleep_for(std::chrono::seconds(5)); +} From 9501b07d74e66a3b0c8f5c5a59db28c009bc9a2b Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 23 Oct 2023 17:46:44 -0600 Subject: [PATCH 4/9] make_child_logger with tests --- .../utils/include/moveit/utils/logger.hpp | 2 +- moveit_core/utils/src/logger.cpp | 15 ++++- moveit_core/utils/test/CMakeLists.txt | 30 +++++++--- .../{logger_test_node.cpp => logger_dut.cpp} | 28 ++-------- .../utils/test/logger_from_child_dut.cpp | 56 +++++++++++++++++++ ..._launch_test.py => rosout_publish_test.py} | 27 +++------ 6 files changed, 106 insertions(+), 52 deletions(-) rename moveit_core/utils/test/{logger_test_node.cpp => logger_dut.cpp} (69%) create mode 100644 moveit_core/utils/test/logger_from_child_dut.cpp rename moveit_core/utils/test/{logger_launch_test.py => rosout_publish_test.py} (81%) diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index b61f1252c5..4b0b1ff2ff 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -49,7 +49,7 @@ const rclcpp::Logger& get_logger(); // Function for getting a child logger. In Humble this also creates a node // Do no use this in place as it will create a new logger each time // instead store it in the state of your class or method. -rclcpp::Logger get_child_logger(const std::string& name); +rclcpp::Logger make_child_logger(const std::string& name); // Mutable access to global logger for setting to node logger rclcpp::Logger& get_logger_mut(); diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index 8f5a593fa6..d89bfe7c76 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace moveit { @@ -48,8 +49,20 @@ const rclcpp::Logger& get_logger() return get_logger_mut(); } -rclcpp::Logger get_child_logger(const std::string& name) +rclcpp::Logger make_child_logger(const std::string& name) { + // On versions of ROS older than Iron we need to create a node for each child logger + // Remove once Humble is EOL + // References: + // Use parent logger (rcl PR) - https://github.com/ros2/rcl/pull/921 + // Request for backport (rclpy issue) - https://github.com/ros2/rclpy/issues/1131 + // MoveIt PR that added this - https://github.com/ros-planning/moveit2/pull/2445 +#if !RCLCPP_VERSION_GTE(21, 0, 3) + static std::vector> child_nodes; + std::string ns = get_logger().get_name(); + child_nodes.push_back(std::make_shared(name, ns)); +#endif + auto logger = get_logger_mut().get_child(name); return logger; } diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt index c4a5ca29fd..bb52a7e292 100644 --- a/moveit_core/utils/test/CMakeLists.txt +++ b/moveit_core/utils/test/CMakeLists.txt @@ -1,11 +1,25 @@ +# Build devices under test +add_executable(logger_dut logger_dut.cpp) +target_link_libraries(logger_dut rclcpp::rclcpp moveit_utils) -find_package(launch_testing_ament_cmake) - -add_executable(logger_test_node logger_test_node.cpp) -target_link_libraries(logger_test_node rclcpp::rclcpp moveit_utils) -install(TARGETS logger_test_node - DESTINATION lib/${PROJECT_NAME}) +add_executable(logger_from_child_dut logger_from_child_dut.cpp) +target_link_libraries(logger_from_child_dut rclcpp::rclcpp moveit_utils) +# Install is needed to run these as launch tests +install( + TARGETS + logger_dut + logger_from_child_dut + DESTINATION lib/${PROJECT_NAME} +) -# Runs logget_test_node and observes ros logging -add_launch_test(logger_launch_test.py) +# Add the launch tests +find_package(launch_testing_ament_cmake) +add_launch_test(rosout_publish_test.py + TARGET test-node_logging + ARGS "dut:=logger_dut" +) +add_launch_test(rosout_publish_test.py + TARGET test-node_logging_from_child + ARGS "dut:=logger_from_child_dut" +) diff --git a/moveit_core/utils/test/logger_test_node.cpp b/moveit_core/utils/test/logger_dut.cpp similarity index 69% rename from moveit_core/utils/test/logger_test_node.cpp rename to moveit_core/utils/test/logger_dut.cpp index a73635e236..ca1525f1fc 100644 --- a/moveit_core/utils/test/logger_test_node.cpp +++ b/moveit_core/utils/test/logger_dut.cpp @@ -36,37 +36,19 @@ #include #include -#include #include -#include -#include int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("logger_test_node"); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); - // Not a node logger, should print but should not be in file or rosout output - RCLCPP_INFO(moveit::get_logger(), "Before"); - - // Set the node logger + // Set the moveit logger to be from node moveit::get_logger_mut() = node->get_logger(); // A node logger, should be in the file output and rosout - RCLCPP_INFO(moveit::get_logger(), "After"); - - // A child logger, should also be in the file output and rosout - const auto child_logger = moveit::get_child_logger("child"); - RCLCPP_INFO(child_logger, "Child"); - - // Spin the node to publish to /rosout -#if RCLCPP_VERSION_GTE(22, 1, 0) // https://github.com/ros2/rclcpp/commit/a17d26b20ac41cc9d5bf3583de8475bb7c18bb1e - rclcpp::spin_all(node, std::chrono::seconds(1)); -#else // Humble or Iron - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node); - exec.spin_all(std::chrono::seconds(1)); -#endif + auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), + [&] { RCLCPP_INFO(moveit::get_logger(), "hello from node!"); }); - std::this_thread::sleep_for(std::chrono::seconds(5)); + rclcpp::spin(node); } diff --git a/moveit_core/utils/test/logger_from_child_dut.cpp b/moveit_core/utils/test/logger_from_child_dut.cpp new file mode 100644 index 0000000000..a444d038b7 --- /dev/null +++ b/moveit_core/utils/test/logger_from_child_dut.cpp @@ -0,0 +1,56 @@ +/********************************************************************* + * 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"); + + // Set the moveit logger to be from node + moveit::get_logger_mut() = node->get_logger(); + + // 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 child node!"); }); + rclcpp::spin(node); +} diff --git a/moveit_core/utils/test/logger_launch_test.py b/moveit_core/utils/test/rosout_publish_test.py similarity index 81% rename from moveit_core/utils/test/logger_launch_test.py rename to moveit_core/utils/test/rosout_publish_test.py index 0817470d90..4c1ac9d2e3 100644 --- a/moveit_core/utils/test/logger_launch_test.py +++ b/moveit_core/utils/test/rosout_publish_test.py @@ -32,9 +32,6 @@ # Author: Tyler Weaver -# Based on https://github.com/ros2/launch/blob/master/launch_testing/test/launch_testing/examples/good_proc_launch_test.py - -import os import unittest from threading import Event from threading import Thread @@ -54,17 +51,16 @@ def generate_test_description(): # dut: device under test dut_process = launch_ros.actions.Node( package="moveit_core", - executable="logger_test_node", - name="logger_test_node", + executable=launch.substitutions.LaunchConfiguration("dut"), output="screen", ) - # This is necessary to get unbuffered output from the process under test - proc_env = os.environ.copy() - proc_env["PYTHONUNBUFFERED"] = "1" - return launch.LaunchDescription( [ + launch.actions.DeclareLaunchArgument( + "dut", + description="Executable to launch", + ), dut_process, # Start tests right away - no need to wait for anything launch_testing.actions.ReadyToTest(), @@ -72,11 +68,10 @@ def generate_test_description(): ), {"dut_process": dut_process} -class MakeTestNode(Node): - def __init__(self, name="test_node"): +class MakeRosoutObserverNode(Node): + def __init__(self, name="rosout_observer_node"): super().__init__(name) self.msg_event_object = Event() - self.rosout_msgs = [] def start_subscriber(self): # Create a subscriber @@ -91,22 +86,16 @@ def start_subscriber(self): self.ros_spin_thread.start() def subscriber_callback(self, data): - self.rosout_msgs.append(data) self.msg_event_object.set() # These tests will run concurrently with the dut process. After all these tests are done, # the launch system will shut down the processes that it started up class TestFixture(unittest.TestCase): - def test_stdout_print(self, proc_output): - proc_output.assertWaitFor("Before", timeout=10, stream="stdout") - proc_output.assertWaitFor("After", timeout=10, stream="stdout") - proc_output.assertWaitFor("Child", timeout=10, stream="stdout") - def test_rosout_msgs_published(self, proc_output): rclpy.init() try: - node = MakeTestNode("test_node") + node = MakeRosoutObserverNode() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=5.0) assert msgs_received_flag, "Did not receive msgs !" From affe738d5d3fb92958715c254192aa9f498d034a Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 23 Oct 2023 18:13:27 -0600 Subject: [PATCH 5/9] Use child loggers --- .../moveit/warehouse/constraints_storage.h | 3 +++ .../moveit/warehouse/planning_scene_storage.h | 2 ++ .../warehouse/planning_scene_world_storage.h | 2 ++ .../include/moveit/warehouse/state_storage.h | 2 ++ .../trajectory_constraints_storage.h | 2 ++ .../moveit/warehouse/warehouse_connector.h | 2 ++ .../warehouse/src/constraints_storage.cpp | 9 +++---- moveit_ros/warehouse/src/import_from_text.cpp | 4 +-- .../warehouse/src/planning_scene_storage.cpp | 27 +++++++++---------- .../src/planning_scene_world_storage.cpp | 8 +++--- moveit_ros/warehouse/src/state_storage.cpp | 9 +++---- .../src/trajectory_constraints_storage.cpp | 8 +++--- .../warehouse/src/warehouse_connector.cpp | 13 +++++---- 13 files changed, 50 insertions(+), 41 deletions(-) diff --git a/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h index 6fa5b7bc8e..230b1c8555 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h @@ -42,6 +42,8 @@ #include +#include + namespace moveit_warehouse { typedef warehouse_ros::MessageWithMetadata::ConstPtr ConstraintsWithMetadata; @@ -83,5 +85,6 @@ class MOVEIT_WAREHOUSE_EXPORT ConstraintsStorage : public MoveItMessageStorage void createCollections(); ConstraintsCollection constraints_collection_; + rclcpp::Logger logger_; }; } // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h index a81057df4c..8139c00ea8 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h @@ -41,6 +41,7 @@ #include #include #include +#include #include @@ -119,5 +120,6 @@ class MOVEIT_WAREHOUSE_EXPORT PlanningSceneStorage : public MoveItMessageStorage PlanningSceneCollection planning_scene_collection_; MotionPlanRequestCollection motion_plan_request_collection_; RobotTrajectoryCollection robot_trajectory_collection_; + rclcpp::Logger logger_; }; } // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h index f7aff02de4..4c17459656 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h @@ -38,6 +38,7 @@ #include #include +#include namespace moveit_warehouse { @@ -70,5 +71,6 @@ class PlanningSceneWorldStorage : public MoveItMessageStorage void createCollections(); PlanningSceneWorldCollection planning_scene_world_collection_; + rclcpp::Logger logger_; }; } // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h index dddc411755..58a8c91a70 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h @@ -39,6 +39,7 @@ #include #include #include +#include #include @@ -78,5 +79,6 @@ class MOVEIT_WAREHOUSE_EXPORT RobotStateStorage : public MoveItMessageStorage void createCollections(); RobotStateCollection state_collection_; + rclcpp::Logger logger_; }; } // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h index 8a0d9ba084..e729fac14c 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit_warehouse { @@ -84,5 +85,6 @@ class TrajectoryConstraintsStorage : public MoveItMessageStorage void createCollections(); TrajectoryConstraintsCollection constraints_collection_; + rclcpp::Logger logger_; }; } // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h index 04e279a3ca..86b8e18997 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h @@ -37,6 +37,7 @@ #pragma once #include +#include namespace moveit_warehouse { @@ -52,5 +53,6 @@ class WarehouseConnector private: std::string dbexec_; int child_pid_; + rclcpp::Logger logger_; }; } // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/src/constraints_storage.cpp b/moveit_ros/warehouse/src/constraints_storage.cpp index ccd9e5a15f..ead85bb713 100644 --- a/moveit_ros/warehouse/src/constraints_storage.cpp +++ b/moveit_ros/warehouse/src/constraints_storage.cpp @@ -45,12 +45,11 @@ const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME = "c const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id"; const std::string moveit_warehouse::ConstraintsStorage::ROBOT_NAME = "robot_id"; -using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::ConstraintsStorage::ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::make_child_logger("moveit_warehouse_constraints_storage")) { createCollections(); } @@ -81,7 +80,7 @@ void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::msg metadata->append(ROBOT_NAME, robot); metadata->append(CONSTRAINTS_GROUP_NAME, group); constraints_collection_->insert(msg, metadata); - RCLCPP_DEBUG(get_logger(), "%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str()); + RCLCPP_DEBUG(logger_, "%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str()); } bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string& name, const std::string& robot, @@ -158,7 +157,7 @@ void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string& Metadata::Ptr m = constraints_collection_->createMetadata(); m->append(CONSTRAINTS_ID_NAME, new_name); constraints_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(get_logger(), "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(logger_, "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& name, const std::string& robot, @@ -171,5 +170,5 @@ void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& if (!group.empty()) q->append(CONSTRAINTS_GROUP_NAME, group); unsigned int rem = constraints_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u Constraints messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u Constraints messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index bded956345..4370a106f9 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -47,10 +47,10 @@ #include #include -using moveit::get_logger; - static const std::string ROBOT_DESCRIPTION = "robot_description"; +using moveit::get_logger; + void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* psm, moveit_warehouse::RobotStateStorage* rs) { diff --git a/moveit_ros/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/src/planning_scene_storage.cpp index b3aec0b8ad..439c6c1c21 100644 --- a/moveit_ros/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp @@ -45,12 +45,11 @@ const std::string moveit_warehouse::PlanningSceneStorage::DATABASE_NAME = "movei const std::string moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME = "planning_scene_id"; const std::string moveit_warehouse::PlanningSceneStorage::MOTION_PLAN_REQUEST_ID_NAME = "motion_request_id"; -using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::PlanningSceneStorage::PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::make_child_logger("moveit_warehouse_planning_scene_storage")) { createCollections(); } @@ -85,7 +84,7 @@ void moveit_warehouse::PlanningSceneStorage::addPlanningScene(const moveit_msgs: Metadata::Ptr metadata = planning_scene_collection_->createMetadata(); metadata->append(PLANNING_SCENE_ID_NAME, scene.name); planning_scene_collection_->insert(scene, metadata); - RCLCPP_DEBUG(get_logger(), "%s scene '%s'", replace ? "Replaced" : "Added", scene.name.c_str()); + RCLCPP_DEBUG(logger_, "%s scene '%s'", replace ? "Replaced" : "Added", scene.name.c_str()); } bool moveit_warehouse::PlanningSceneStorage::hasPlanningScene(const std::string& name) const @@ -173,7 +172,7 @@ moveit_warehouse::PlanningSceneStorage::addNewPlanningRequest(const moveit_msgs: metadata->append(PLANNING_SCENE_ID_NAME, scene_name); metadata->append(MOTION_PLAN_REQUEST_ID_NAME, id); motion_plan_request_collection_->insert(planning_query, metadata); - RCLCPP_DEBUG(get_logger(), "Saved planning query '%s' for scene '%s'", id.c_str(), scene_name.c_str()); + RCLCPP_DEBUG(logger_, "Saved planning query '%s' for scene '%s'", id.c_str(), scene_name.c_str()); return id; } @@ -231,7 +230,7 @@ bool moveit_warehouse::PlanningSceneStorage::getPlanningScene(PlanningSceneWithM std::vector planning_scenes = planning_scene_collection_->queryList(q, false); if (planning_scenes.empty()) { - RCLCPP_WARN(get_logger(), "Planning scene '%s' was not found in the database", scene_name.c_str()); + RCLCPP_WARN(logger_, "Planning scene '%s' was not found in the database", scene_name.c_str()); return false; } scene_m = planning_scenes.back(); @@ -251,7 +250,7 @@ bool moveit_warehouse::PlanningSceneStorage::getPlanningQuery(MotionPlanRequestW std::vector planning_queries = motion_plan_request_collection_->queryList(q, false); if (planning_queries.empty()) { - RCLCPP_ERROR(get_logger(), "Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str()); + RCLCPP_ERROR(logger_, "Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str()); return false; } else @@ -369,7 +368,7 @@ void moveit_warehouse::PlanningSceneStorage::renamePlanningScene(const std::stri Metadata::Ptr m = planning_scene_collection_->createMetadata(); m->append(PLANNING_SCENE_ID_NAME, new_scene_name); planning_scene_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(get_logger(), "Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str()); + RCLCPP_DEBUG(logger_, "Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::renamePlanningQuery(const std::string& scene_name, @@ -382,7 +381,7 @@ void moveit_warehouse::PlanningSceneStorage::renamePlanningQuery(const std::stri Metadata::Ptr m = motion_plan_request_collection_->createMetadata(); m->append(MOTION_PLAN_REQUEST_ID_NAME, new_query_name); motion_plan_request_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(get_logger(), "Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), + RCLCPP_DEBUG(logger_, "Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), old_query_name.c_str(), new_query_name.c_str()); } @@ -392,7 +391,7 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningScene(const std::stri Query::Ptr q = planning_scene_collection_->createQuery(); q->append(PLANNING_SCENE_ID_NAME, scene_name); unsigned int rem = planning_scene_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningQueries(const std::string& scene_name) @@ -401,7 +400,7 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningQueries(const std::st Query::Ptr q = motion_plan_request_collection_->createQuery(); q->append(PLANNING_SCENE_ID_NAME, scene_name); unsigned int rem = motion_plan_request_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningQuery(const std::string& scene_name, @@ -412,8 +411,8 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningQuery(const std::stri q->append(PLANNING_SCENE_ID_NAME, scene_name); q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name); unsigned int rem = motion_plan_request_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, - scene_name.c_str(), query_name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, scene_name.c_str(), + query_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string& scene_name) @@ -421,7 +420,7 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::st Query::Ptr q = robot_trajectory_collection_->createQuery(); q->append(PLANNING_SCENE_ID_NAME, scene_name); unsigned int rem = robot_trajectory_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str()); } void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string& scene_name, @@ -431,6 +430,6 @@ void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::st q->append(PLANNING_SCENE_ID_NAME, scene_name); q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name); unsigned int rem = robot_trajectory_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(), + RCLCPP_DEBUG(logger_, "Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(), query_name.c_str()); } diff --git a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp index 9934bbd5bd..4a4e631302 100644 --- a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp @@ -42,12 +42,12 @@ const std::string moveit_warehouse::PlanningSceneWorldStorage::DATABASE_NAME = "moveit_planning_scene_worlds"; const std::string moveit_warehouse::PlanningSceneWorldStorage::PLANNING_SCENE_WORLD_ID_NAME = "world_id"; -using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::PlanningSceneWorldStorage::PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn) : MoveItMessageStorage(std::move(conn)) + , logger_(moveit::make_child_logger("moveit_warehouse_planning_scene_world_storage")) { createCollections(); } @@ -77,7 +77,7 @@ void moveit_warehouse::PlanningSceneWorldStorage::addPlanningSceneWorld(const mo Metadata::Ptr metadata = planning_scene_world_collection_->createMetadata(); metadata->append(PLANNING_SCENE_WORLD_ID_NAME, name); planning_scene_world_collection_->insert(msg, metadata); - RCLCPP_DEBUG(get_logger(), "%s planning scene world '%s'", replace ? "Replaced" : "Added", name.c_str()); + RCLCPP_DEBUG(logger_, "%s planning scene world '%s'", replace ? "Replaced" : "Added", name.c_str()); } bool moveit_warehouse::PlanningSceneWorldStorage::hasPlanningSceneWorld(const std::string& name) const @@ -133,7 +133,7 @@ void moveit_warehouse::PlanningSceneWorldStorage::renamePlanningSceneWorld(const Metadata::Ptr m = planning_scene_world_collection_->createMetadata(); m->append(PLANNING_SCENE_WORLD_ID_NAME, new_name); planning_scene_world_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(get_logger(), "Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(logger_, "Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::PlanningSceneWorldStorage::removePlanningSceneWorld(const std::string& name) @@ -141,5 +141,5 @@ void moveit_warehouse::PlanningSceneWorldStorage::removePlanningSceneWorld(const Query::Ptr q = planning_scene_world_collection_->createQuery(); q->append(PLANNING_SCENE_WORLD_ID_NAME, name); unsigned int rem = planning_scene_world_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u PlanningSceneWorld messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u PlanningSceneWorld messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/state_storage.cpp b/moveit_ros/warehouse/src/state_storage.cpp index d5c7ad205e..d85c4c9eb1 100644 --- a/moveit_ros/warehouse/src/state_storage.cpp +++ b/moveit_ros/warehouse/src/state_storage.cpp @@ -44,12 +44,11 @@ const std::string moveit_warehouse::RobotStateStorage::DATABASE_NAME = "moveit_r const std::string moveit_warehouse::RobotStateStorage::STATE_NAME = "state_id"; const std::string moveit_warehouse::RobotStateStorage::ROBOT_NAME = "robot_id"; -using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::RobotStateStorage::RobotStateStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::make_child_logger("moveit_warehouse_robot_state_storage")) { createCollections(); } @@ -79,7 +78,7 @@ void moveit_warehouse::RobotStateStorage::addRobotState(const moveit_msgs::msg:: metadata->append(STATE_NAME, name); metadata->append(ROBOT_NAME, robot); state_collection_->insert(msg, metadata); - RCLCPP_DEBUG(get_logger(), "%s robot state '%s'", replace ? "Replaced" : "Added", name.c_str()); + RCLCPP_DEBUG(logger_, "%s robot state '%s'", replace ? "Replaced" : "Added", name.c_str()); } bool moveit_warehouse::RobotStateStorage::hasRobotState(const std::string& name, const std::string& robot) const @@ -143,7 +142,7 @@ void moveit_warehouse::RobotStateStorage::renameRobotState(const std::string& ol Metadata::Ptr m = state_collection_->createMetadata(); m->append(STATE_NAME, new_name); state_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(get_logger(), "Renamed robot state from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(logger_, "Renamed robot state from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::RobotStateStorage::removeRobotState(const std::string& name, const std::string& robot) @@ -153,5 +152,5 @@ void moveit_warehouse::RobotStateStorage::removeRobotState(const std::string& na if (!robot.empty()) q->append(ROBOT_NAME, robot); unsigned int rem = state_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u RobotState messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u RobotState messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp index fcc8744db0..79e59d350c 100644 --- a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp @@ -45,12 +45,12 @@ const std::string moveit_warehouse::TrajectoryConstraintsStorage::CONSTRAINTS_ID const std::string moveit_warehouse::TrajectoryConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id"; const std::string moveit_warehouse::TrajectoryConstraintsStorage::ROBOT_NAME = "robot_id"; -using moveit::get_logger; using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) : MoveItMessageStorage(std::move(conn)) + , logger_(moveit::make_child_logger("moveit_warehouse_trajectory_constraints_storage")) { createCollections(); } @@ -83,7 +83,7 @@ void moveit_warehouse::TrajectoryConstraintsStorage::addTrajectoryConstraints( metadata->append(ROBOT_NAME, robot); metadata->append(CONSTRAINTS_GROUP_NAME, group); constraints_collection_->insert(msg, metadata); - RCLCPP_DEBUG(get_logger(), "%s constraints '%s'", replace ? "Replaced" : "Added", name.c_str()); + RCLCPP_DEBUG(logger_, "%s constraints '%s'", replace ? "Replaced" : "Added", name.c_str()); } bool moveit_warehouse::TrajectoryConstraintsStorage::hasTrajectoryConstraints(const std::string& name, @@ -165,7 +165,7 @@ void moveit_warehouse::TrajectoryConstraintsStorage::renameTrajectoryConstraints Metadata::Ptr m = constraints_collection_->createMetadata(); m->append(CONSTRAINTS_ID_NAME, new_name); constraints_collection_->modifyMetadata(q, m); - RCLCPP_DEBUG(get_logger(), "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); + RCLCPP_DEBUG(logger_, "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str()); } void moveit_warehouse::TrajectoryConstraintsStorage::removeTrajectoryConstraints(const std::string& name, @@ -179,5 +179,5 @@ void moveit_warehouse::TrajectoryConstraintsStorage::removeTrajectoryConstraints if (!group.empty()) q->append(CONSTRAINTS_GROUP_NAME, group); unsigned int rem = constraints_collection_->removeMessages(q); - RCLCPP_DEBUG(get_logger(), "Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str()); + RCLCPP_DEBUG(logger_, "Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str()); } diff --git a/moveit_ros/warehouse/src/warehouse_connector.cpp b/moveit_ros/warehouse/src/warehouse_connector.cpp index b87590b16e..99c9e2828a 100644 --- a/moveit_ros/warehouse/src/warehouse_connector.cpp +++ b/moveit_ros/warehouse/src/warehouse_connector.cpp @@ -42,16 +42,14 @@ #include #include -using moveit::get_logger; - #ifdef _WIN32 void kill(int, int) { - RCLCPP_ERROR(get_logger(), "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(moveit::get_logger(), "Warehouse connector not supported on Windows"); } // Should never be called int fork() { - RCLCPP_ERROR(get_logger(), "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(moveit::get_logger(), "Warehouse connector not supported on Windows"); return -1; } #else @@ -60,7 +58,8 @@ int fork() namespace moveit_warehouse { -WarehouseConnector::WarehouseConnector(const std::string& dbexec) : dbexec_(dbexec), child_pid_(0) +WarehouseConnector::WarehouseConnector(const std::string& dbexec) + : dbexec_(dbexec), child_pid_(0), logger_(moveit::make_child_logger("moveit_warehouse_warehouse_connector")) { } @@ -78,7 +77,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname) child_pid_ = fork(); if (child_pid_ == -1) { - RCLCPP_ERROR(get_logger(), "Error forking process."); + RCLCPP_ERROR(logger_, "Error forking process."); child_pid_ = 0; return false; } @@ -106,7 +105,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname) delete[] argv[1]; delete[] argv[2]; delete[] argv; - RCLCPP_ERROR_STREAM(get_logger(), + RCLCPP_ERROR_STREAM(logger_, "execv() returned " << code << ", errno=" << errno << " string errno = " << strerror(errno)); } return false; From 150da604cbb82f97f02df63e62f2a89c09713c99 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 24 Oct 2023 08:10:42 -0600 Subject: [PATCH 6/9] Update moveit_core/utils/include/moveit/utils/logger.hpp Co-authored-by: Henning Kayser --- moveit_core/utils/include/moveit/utils/logger.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 4b0b1ff2ff..7ef63c8362 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -14,7 +14,7 @@ * 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 + * * Neither the name of PickNik Robotics Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * From f0ec87250f26ba6fcd9e658908138d3060f4d1d3 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 24 Oct 2023 08:14:13 -0600 Subject: [PATCH 7/9] Update moveit_core/utils/src/logger.cpp --- moveit_core/utils/src/logger.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index d89bfe7c76..f0cd880d74 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -63,8 +63,7 @@ rclcpp::Logger make_child_logger(const std::string& name) child_nodes.push_back(std::make_shared(name, ns)); #endif - auto logger = get_logger_mut().get_child(name); - return logger; + return get_logger_mut().get_child(name); } // Mutable access to global logger for setting to node logger From e1b919d35b51d50334eaa5c51cfbb3336bf113cf Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 24 Oct 2023 09:51:22 -0600 Subject: [PATCH 8/9] Use unordered map instead of vector to avoid re-creating nodes Signed-off-by: Tyler Weaver --- moveit_core/utils/include/moveit/utils/logger.hpp | 10 +++++----- moveit_core/utils/src/logger.cpp | 15 ++++++++------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 7ef63c8362..40c88093c7 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -42,16 +42,16 @@ namespace moveit { -// Function for getting a reference to a logger object kept on the stack -// Use get_logger_mut to set the logger to a node logger +// 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. const rclcpp::Logger& get_logger(); -// Function for getting a child logger. In Humble this also creates a node -// Do no use this in place as it will create a new logger each time +// Function for getting a child logger. In Humble this also creates a node. +// Do no use this in place as it will create a new logger each time, // instead store it in the state of your class or method. rclcpp::Logger make_child_logger(const std::string& name); -// Mutable access to global logger for setting to node logger +// Mutable access to global logger for setting to node logger. rclcpp::Logger& get_logger_mut(); } // namespace moveit diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp index f0cd880d74..a6a314f160 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -37,13 +37,12 @@ #include #include #include -#include +#include +#include namespace moveit { -// Function for getting a reference to a logger object kept on the stack -// Use get_logger_mut to set the logger to a node logger const rclcpp::Logger& get_logger() { return get_logger_mut(); @@ -58,15 +57,17 @@ rclcpp::Logger make_child_logger(const std::string& name) // Request for backport (rclpy issue) - https://github.com/ros2/rclpy/issues/1131 // MoveIt PR that added this - https://github.com/ros-planning/moveit2/pull/2445 #if !RCLCPP_VERSION_GTE(21, 0, 3) - static std::vector> child_nodes; - std::string ns = get_logger().get_name(); - child_nodes.push_back(std::make_shared(name, ns)); + static std::unordered_map> child_nodes; + if (child_nodes.find(name) == child_nodes.end()) + { + std::string ns = get_logger().get_name(); + child_nodes[name] = std::make_shared(name, ns); + } #endif return get_logger_mut().get_child(name); } -// Mutable access to global logger for setting to node logger rclcpp::Logger& get_logger_mut() { static rclcpp::Logger logger = rclcpp::get_logger("moveit"); From 047dbd62cf7b358c7cdfcfbda87a1b97a576ed0d Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 24 Oct 2023 10:17:43 -0600 Subject: [PATCH 9/9] Update moveit_core/utils/include/moveit/utils/logger.hpp Co-authored-by: Henning Kayser --- moveit_core/utils/include/moveit/utils/logger.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 40c88093c7..7d3039f02b 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -47,7 +47,7 @@ namespace moveit const rclcpp::Logger& get_logger(); // Function for getting a child logger. In Humble this also creates a node. -// Do no use this in place as it will create a new logger each time, +// Do not use this in place as it will create a new logger each time, // instead store it in the state of your class or method. rclcpp::Logger make_child_logger(const std::string& name);