Skip to content

Commit

Permalink
fix logging for ROS2
Browse files Browse the repository at this point in the history
  • Loading branch information
Cakem1x committed Dec 19, 2023
1 parent 84be9fc commit eb78040
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 17 deletions.
12 changes: 8 additions & 4 deletions mesh_client/include/mesh_client/mesh_client.h
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@

#include <string>
#include <array>
#include <rclcpp/logger.hpp>
#include <lvr2/io/AttributeMeshIOBase.hpp>
#include <lvr2/geometry/BoundingBox.hpp>
#include <lvr2/geometry/BaseVector.hpp>
Expand All @@ -53,11 +54,12 @@ class MeshClient : public lvr2::AttributeMeshIOBase
{
public:
/**
* @brief Constructs a mesh client which receaves
* @param srv_url The url of the server, use http://localhost/my/path/to/mesh if the server is used locally.
* @brief Constructs a mesh client which receives
* @param server_url The url of the server, use http://localhost/my/path/to/mesh if the server is used locally.
* @param logger The ROS logger that shall be used by the MeshClient. Defaults to using a logger "mesh_client"
*/
MeshClient(const std::string& srv_url, const std::string& server_username, const std::string& server_password,
const std::string& mesh_layer);
MeshClient(const std::string& server_url, const std::string& server_username, const std::string& server_password,
const std::string& mesh_layer, rclcpp::Logger logger = rclcpp::get_logger("mesh_client"));

/**
* @brief sets the Bounding box for the query which is send to the server
Expand Down Expand Up @@ -176,6 +178,8 @@ class MeshClient : public lvr2::AttributeMeshIOBase
std::string mesh_layer_;
std::array<float, 6> bounding_box_;
std::map<std::string, std::pair<float, float>> mesh_filters_;

rclcpp::Logger logger_;
};

size_t writeFunction(void* ptr, size_t size, size_t nmemb, std::string* data)
Expand Down
26 changes: 13 additions & 13 deletions mesh_client/src/mesh_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,16 +37,16 @@
#include "mesh_client/mesh_client.h"

#include <curl/curl.h>
#include <ros/ros.h>
#include <rclcpp/rclcpp.hpp>

namespace mesh_client
{
MeshClient::MeshClient(const std::string& server_url, const std::string& server_username,
const std::string& server_password, const std::string& mesh_layer)
MeshClient::MeshClient(const std::string& server_url, const std::string& server_username, const std::string& server_password, const std::string& mesh_layer, rclcpp::Logger logger)
: server_url_(server_url)
, server_username_(server_username)
, server_password_(server_password)
, mesh_layer_(mesh_layer)
, logger_(logger)
{
}

Expand Down Expand Up @@ -135,7 +135,7 @@ lvr2::FloatChannelOptional MeshClient::getVertices()
unsigned long size, width;
char* data;

ROS_DEBUG_STREAM("Received vertices channel");
RCLCPP_DEBUG_STREAM(logger_, "Received vertices channel");

if (parseByteDataString(*str, type, size, width, data) && type == Type::FLOAT)
{
Expand All @@ -146,7 +146,7 @@ lvr2::FloatChannelOptional MeshClient::getVertices()
return channel;
}
}
ROS_ERROR_STREAM("Failed to load vertices channel!");
RCLCPP_ERROR_STREAM(logger_, "Failed to load vertices channel!");
return lvr2::FloatChannelOptional();
}

Expand All @@ -165,7 +165,7 @@ lvr2::IndexChannelOptional MeshClient::getIndices()
unsigned long size, width;
char* data;

ROS_DEBUG_STREAM("Received indices channel");
RCLCPP_DEBUG_STREAM(logger_, "Received indices channel");

if (parseByteDataString(*str, type, size, width, data) && type == Type::UINT)
{
Expand All @@ -176,7 +176,7 @@ lvr2::IndexChannelOptional MeshClient::getIndices()
return channel;
}
}
ROS_ERROR_STREAM("Failed to load indices channel!");
RCLCPP_ERROR_STREAM(logger_, "Failed to load indices channel!");
return lvr2::IndexChannelOptional();
}

Expand Down Expand Up @@ -209,7 +209,7 @@ bool MeshClient::getChannel(const std::string group, const std::string name, lvr
unsigned long size, width;
char* data;

ROS_DEBUG_STREAM("Received " << name << " channel");
RCLCPP_DEBUG_STREAM(logger_, "Received " << name << " channel");

if (parseByteDataString(*str, type, size, width, data) && type == Type::FLOAT)
{
Expand All @@ -219,7 +219,7 @@ bool MeshClient::getChannel(const std::string group, const std::string name, lvr
return true;
}
}
ROS_ERROR_STREAM("Failed to load " << name << " channel!");
RCLCPP_ERROR_STREAM(logger_, "Failed to load " << name << " channel!");
return false;
}

Expand All @@ -239,7 +239,7 @@ bool MeshClient::getChannel(const std::string group, const std::string name, lvr
unsigned long size, width;
char* data;

ROS_DEBUG_STREAM("Received " << name << " channel");
RCLCPP_DEBUG_STREAM(logger_, "Received " << name << " channel");

if (parseByteDataString(*str, type, size, width, data) && type == Type::UINT)
{
Expand All @@ -249,7 +249,7 @@ bool MeshClient::getChannel(const std::string group, const std::string name, lvr
return true;
}
}
ROS_ERROR_STREAM("Failed to load " << name << " channel!");
RCLCPP_ERROR_STREAM(logger_, "Failed to load " << name << " channel!");
return false;
}

Expand All @@ -269,7 +269,7 @@ bool MeshClient::getChannel(const std::string group, const std::string name, lvr
unsigned long size, width;
char* data;

ROS_DEBUG_STREAM("Received " << name << " channel");
RCLCPP_DEBUG_STREAM(logger_, "Received " << name << " channel");

if (parseByteDataString(*str, type, size, width, data) && type == Type::UINT)
{
Expand All @@ -279,7 +279,7 @@ bool MeshClient::getChannel(const std::string group, const std::string name, lvr
return true;
}
}
ROS_ERROR_STREAM("Failed to load " << name << " channel!");
RCLCPP_ERROR_STREAM(logger_, "Failed to load " << name << " channel!");
return false;
}

Expand Down

0 comments on commit eb78040

Please sign in to comment.