Skip to content

Commit

Permalink
Use node logging in warehouse warehouse_services
Browse files Browse the repository at this point in the history
  • Loading branch information
@tylerjw
tylerjw committed Oct 11, 2023
1 parent e7577ca commit c6cfa42
Showing 1 changed file with 20 additions and 21 deletions.
41 changes: 20 additions & 21 deletions moveit_ros/warehouse/src/warehouse_services.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,15 +51,13 @@

static const std::string ROBOT_DESCRIPTION = "robot_description";

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.warehouse_services");

bool storeState(const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Response>& response,
moveit_warehouse::RobotStateStorage& rs)
moveit_warehouse::RobotStateStorage& rs, const rclcpp::Logger& logger)
{
if (request->name.empty())
{
RCLCPP_ERROR(LOGGER, "You must specify a name to store a state");
RCLCPP_ERROR(logger, "You must specify a name to store a state");
return (response->success = false);
}
rs.addRobotState(request->state, request->name, request->robot);
Expand Down Expand Up @@ -91,11 +89,11 @@ bool hasState(const std::shared_ptr<moveit_msgs::srv::CheckIfRobotStateExistsInW

bool getState(const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Response>& response,
moveit_warehouse::RobotStateStorage& rs)
moveit_warehouse::RobotStateStorage& rs, const rclcpp::Logger& logger)
{
if (!rs.hasRobotState(request->name, request->robot))
{
RCLCPP_ERROR_STREAM(LOGGER, "No state called '" << request->name << "' for robot '" << request->robot << "'.");
RCLCPP_ERROR_STREAM(logger, "No state called '" << request->name << "' for robot '" << request->robot << "'.");
moveit_msgs::msg::RobotState dummy;
response->state = dummy;
return false;
Expand All @@ -108,11 +106,11 @@ bool getState(const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse

bool renameState(const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Response>& /*response*/,
moveit_warehouse::RobotStateStorage& rs)
moveit_warehouse::RobotStateStorage& rs, const rclcpp::Logger& logger)
{
if (!rs.hasRobotState(request->old_name, request->robot))
{
RCLCPP_ERROR_STREAM(LOGGER, "No state called '" << request->old_name << "' for robot '" << request->robot << "'.");
RCLCPP_ERROR_STREAM(logger, "No state called '" << request->old_name << "' for robot '" << request->robot << "'.");
return false;
}
rs.renameRobotState(request->old_name, request->new_name, request->robot);
Expand All @@ -121,11 +119,11 @@ bool renameState(const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWareh

bool deleteState(const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Response>& /*response*/,
moveit_warehouse::RobotStateStorage& rs)
moveit_warehouse::RobotStateStorage& rs, const rclcpp::Logger& logger)
{
if (!rs.hasRobotState(request->name, request->robot))
{
RCLCPP_ERROR_STREAM(LOGGER, "No state called '" << request->name << "' for robot '" << request->robot << "'.");
RCLCPP_ERROR_STREAM(logger, "No state called '" << request->name << "' for robot '" << request->robot << "'.");
return false;
}
rs.removeRobotState(request->name, request->robot);
Expand All @@ -139,6 +137,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);
const auto logger = node->get_logger();

std::string host;

Expand All @@ -158,23 +157,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(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(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(logger, "Failed to connect too many times, giving up");
return 1;
}
}
}
catch (std::exception& ex)
{
RCLCPP_ERROR(LOGGER, "%s", ex.what());
RCLCPP_ERROR(logger, "%s", ex.what());
return 1;
}

Expand All @@ -184,18 +183,18 @@ int main(int argc, char** argv)
rs.getKnownRobotStates(names);
if (names.empty())
{
RCLCPP_INFO(LOGGER, "There are no previously stored robot states");
RCLCPP_INFO(logger, "There are no previously stored robot states");
}
else
{
RCLCPP_INFO(LOGGER, "Previously stored robot states:");
RCLCPP_INFO(logger, "Previously stored robot states:");
for (const std::string& name : names)
RCLCPP_INFO(LOGGER, " * %s", name.c_str());
RCLCPP_INFO(logger, " * %s", name.c_str());
}

auto save_cb = [&](const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Response>& response) -> bool {
return storeState(request, response, rs);
return storeState(request, response, rs, logger);
};

auto list_cb = [&](const std::shared_ptr<moveit_msgs::srv::ListRobotStatesInWarehouse::Request>& request,
Expand All @@ -205,7 +204,7 @@ int main(int argc, char** argv)

auto get_cb = [&](const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Response>& response) -> bool {
return getState(request, response, rs);
return getState(request, response, rs, logger);
};

auto has_cb =
Expand All @@ -217,13 +216,13 @@ int main(int argc, char** argv)
auto rename_cb =
[&](const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Response>& response) -> bool {
return renameState(request, response, rs);
return renameState(request, response, rs, logger);
};

auto delete_cb =
[&](const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Request>& request,
const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Response>& response) -> bool {
return deleteState(request, response, rs);
return deleteState(request, response, rs, logger);
};

auto save_state_server =
Expand Down

0 comments on commit c6cfa42

Please sign in to comment.