diff --git a/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h index 6fa5b7bc8ec..230b1c85556 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 a81057df4cb..8139c00ea88 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 f7aff02de42..4c174596566 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 dddc4117559..58a8c91a703 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 8a0d9ba0845..e729fac14ce 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 04e279a3ca6..86b8e189977 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 ccd9e5a15ff..ead85bb713c 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 bded956345f..4370a106f91 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 b3aec0b8ad5..439c6c1c216 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 9934bbd5bdc..4a4e6313026 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 d5c7ad205e5..d85c4c9eb1e 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 fcc8744db07..79e59d350c8 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 b87590b16e4..99c9e2828a1 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;