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 7be8dd822d..a206662a19 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,10 @@ ament_target_dependencies(moveit_test_utils
srdfdom
urdfdom
urdfdom_headers
+ 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
new file mode 100644
index 0000000000..7d3039f02b
--- /dev/null
+++ b/moveit_core/utils/include/moveit/utils/logger.hpp
@@ -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 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.
+ *
+ * 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
+#include
+
+namespace moveit
+{
+
+// Function for getting a reference to a logger object kept in a global variable.
+// Use get_logger_mut to set the logger to a node logger.
+const rclcpp::Logger& get_logger();
+
+// Function for getting a child logger. In Humble this also creates a node.
+// 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);
+
+// 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..a6a314f160
--- /dev/null
+++ b/moveit_core/utils/src/logger.cpp
@@ -0,0 +1,77 @@
+/*********************************************************************
+ * 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
+
+namespace moveit
+{
+
+const rclcpp::Logger& get_logger()
+{
+ return get_logger_mut();
+}
+
+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::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);
+}
+
+rclcpp::Logger& get_logger_mut()
+{
+ static rclcpp::Logger logger = rclcpp::get_logger("moveit");
+ return logger;
+}
+
+} // namespace moveit
diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt
new file mode 100644
index 0000000000..bb52a7e292
--- /dev/null
+++ b/moveit_core/utils/test/CMakeLists.txt
@@ -0,0 +1,25 @@
+# Build devices under test
+add_executable(logger_dut logger_dut.cpp)
+target_link_libraries(logger_dut rclcpp::rclcpp moveit_utils)
+
+add_executable(logger_from_child_dut logger_from_child_dut.cpp)
+target_link_libraries(logger_from_child_dut rclcpp::rclcpp moveit_utils)
+
+# Install is needed to run these as launch tests
+install(
+ TARGETS
+ logger_dut
+ logger_from_child_dut
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+# 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_dut.cpp b/moveit_core/utils/test/logger_dut.cpp
new file mode 100644
index 0000000000..ca1525f1fc
--- /dev/null
+++ b/moveit_core/utils/test/logger_dut.cpp
@@ -0,0 +1,54 @@
+/*********************************************************************
+ * 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();
+
+ // A node logger, should be in the file output and rosout
+ auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100),
+ [&] { RCLCPP_INFO(moveit::get_logger(), "hello from node!"); });
+
+ 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/rosout_publish_test.py b/moveit_core/utils/test/rosout_publish_test.py
new file mode 100644
index 0000000000..4c1ac9d2e3
--- /dev/null
+++ b/moveit_core/utils/test/rosout_publish_test.py
@@ -0,0 +1,111 @@
+# 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
+
+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=launch.substitutions.LaunchConfiguration("dut"),
+ output="screen",
+ )
+
+ 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(),
+ ]
+ ), {"dut_process": dut_process}
+
+
+class MakeRosoutObserverNode(Node):
+ def __init__(self, name="rosout_observer_node"):
+ super().__init__(name)
+ self.msg_event_object = Event()
+
+ 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.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_rosout_msgs_published(self, proc_output):
+ rclpy.init()
+ try:
+ node = MakeRosoutObserverNode()
+ 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_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/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..ead85bb713 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,13 +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";
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.constraints_storage");
-
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(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(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(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 6b91501433..4370a106f9 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";
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.planning_scene_storage");
+using moveit::get_logger;
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..439c6c1c21 100644
--- a/moveit_ros/warehouse/src/planning_scene_storage.cpp
+++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp
@@ -38,6 +38,7 @@
#include
#include
#include
+#include
const std::string moveit_warehouse::PlanningSceneStorage::DATABASE_NAME = "moveit_planning_scenes";
@@ -47,10 +48,8 @@ const std::string moveit_warehouse::PlanningSceneStorage::MOTION_PLAN_REQUEST_ID
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))
+ : 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(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(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(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(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(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(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(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(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,7 +411,7 @@ 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(),
+ RCLCPP_DEBUG(logger_, "Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, scene_name.c_str(),
query_name.c_str());
}
@@ -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(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(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 310e060f56..4a4e631302 100644
--- a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp
+++ b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp
@@ -35,19 +35,19 @@
/* 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 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(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(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(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/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..d85c4c9eb1 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,13 +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";
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.state_storage");
-
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(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(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(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 8ba11a3ba1..79e59d350c 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,13 +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";
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.trajectory_constraints_storage");
-
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(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(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(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 f75123c50b..99c9e2828a 100644
--- a/moveit_ros/warehouse/src/warehouse_connector.cpp
+++ b/moveit_ros/warehouse/src/warehouse_connector.cpp
@@ -40,17 +40,16 @@
#include
#include
#include
-
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.warehouse_connector");
+#include
#ifdef _WIN32
void kill(int, int)
{
- RCLCPP_ERROR(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(LOGGER, "Warehouse connector not supported on Windows");
+ RCLCPP_ERROR(moveit::get_logger(), "Warehouse connector not supported on Windows");
return -1;
}
#else
@@ -59,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"))
{
}
@@ -77,7 +77,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname)
child_pid_ = fork();
if (child_pid_ == -1)
{
- RCLCPP_ERROR(LOGGER, "Error forking process.");
+ RCLCPP_ERROR(logger_, "Error forking process.");
child_pid_ = 0;
return false;
}
@@ -105,7 +105,7 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname)
delete[] argv[1];
delete[] argv[2];
delete[] argv;
- RCLCPP_ERROR_STREAM(LOGGER,
+ RCLCPP_ERROR_STREAM(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,