Skip to content

Commit

Permalink
Node logger through singleton
Browse files Browse the repository at this point in the history
  • Loading branch information
tylerjw committed Oct 23, 2023
1 parent 15d9d61 commit 9f58e96
Show file tree
Hide file tree
Showing 15 changed files with 231 additions and 105 deletions.
4 changes: 3 additions & 1 deletion moveit_core/utils/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/moveit_core>
)
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)
Expand All @@ -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}")
51 changes: 51 additions & 0 deletions moveit_core/utils/include/moveit/utils/logger.hpp
Original file line number Diff line number Diff line change
@@ -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 <rclcpp/rclcpp.hpp>

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
57 changes: 57 additions & 0 deletions moveit_core/utils/src/logger.cpp
Original file line number Diff line number Diff line change
@@ -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 <rclcpp/rclcpp.hpp>
#include <moveit/utils/logger.hpp>

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
16 changes: 9 additions & 7 deletions moveit_ros/warehouse/src/broadcast.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <moveit/warehouse/planning_scene_storage.h>
#include <moveit/warehouse/constraints_storage.h>
#include <moveit/warehouse/state_storage.h>
#include <moveit/utils/logger.hpp>
#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
Expand Down Expand Up @@ -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)
{
Expand All @@ -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;
Expand Down Expand Up @@ -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<const moveit_msgs::msg::PlanningScene&>(*pswm));
executor.spin_once(0ns);
Expand All @@ -153,14 +155,14 @@ int main(int argc, char** argv)
std::vector<moveit_warehouse::MotionPlanRequestWithMetadata> planning_queries;
std::vector<std::string> 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<int>(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<const moveit_msgs::msg::MotionPlanRequest&>(*planning_queries[i]));
executor.spin_once(0ns);
}
Expand Down Expand Up @@ -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<const moveit_msgs::msg::Constraints&>(*cwm));
executor.spin_once(0ns);
Expand All @@ -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<const moveit_msgs::msg::RobotState&>(*rswm));
executor.spin_once(0ns);
Expand All @@ -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;
}
10 changes: 5 additions & 5 deletions moveit_ros/warehouse/src/constraints_storage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
/* Author: Ioan Sucan */

#include <moveit/warehouse/constraints_storage.h>
#include <moveit/utils/logger.hpp>

#include <utility>

Expand All @@ -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;

Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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());
}
18 changes: 10 additions & 8 deletions moveit_ros/warehouse/src/import_from_text.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,11 @@
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <moveit/utils/logger.hpp>

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)
Expand Down Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -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;
}

Expand All @@ -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);
}
}
Expand Down Expand Up @@ -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());
}
}
}
Expand Down Expand Up @@ -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());
}
}
}
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}

Expand Down
14 changes: 8 additions & 6 deletions moveit_ros/warehouse/src/initialize_demo_db.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,11 @@
#include <rclcpp/node.hpp>
#include <rclcpp/node_options.hpp>
#include <rclcpp/utilities.hpp>
#include <moveit/utils/logger.hpp>

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)
{
Expand All @@ -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<std::string>(),
Expand All @@ -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;
}

Expand All @@ -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<std::string>& gnames = psm.getRobotModel()->getJointModelGroupNames();
for (const std::string& gname : gnames)
Expand All @@ -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);

Expand Down
Loading

0 comments on commit 9f58e96

Please sign in to comment.