Skip to content
This repository has been archived by the owner on Jul 22, 2021. It is now read-only.

rclcpp requires const char * for logging macros #86

Merged
merged 1 commit into from
Jan 14, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 17 additions & 11 deletions rmf_schedule_visualizer/src/Server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,8 @@ Server::Server(uint16_t port,
rmf_traffic::schedule::Negotiation::Table::ViewerPtr table_view)
{
RCLCPP_DEBUG(_visualizer_data_node->get_logger(),
"======== conflict callback version: %llu! ==========",
std::string("======== conflict callback "
"version: %llu! ==========").c_str(),
conflict_version);

nlohmann::json negotiation_json;
Expand Down Expand Up @@ -98,7 +99,8 @@ Server::Server(uint16_t port,
uint64_t conflict_version, bool resolved)
{
RCLCPP_DEBUG(_visualizer_data_node->get_logger(),
"======== conflict concluded: %llu resolved: %d ==========",
std::string("======== conflict concluded: "
"%llu resolved: %d ==========").c_str(),
conflict_version, resolved ? 1 : 0);

nlohmann::json json_msg;
Expand All @@ -118,15 +120,15 @@ void Server::on_open(connection_hdl hdl)
{
_connections.insert(hdl);
RCLCPP_INFO(_visualizer_data_node->get_logger(),
"Connected with a client");
std::string("Connected with a client").c_str());
}

void Server::on_close(connection_hdl hdl)
{
_connections.erase(hdl);
_negotiation_subscribed_connections.erase(hdl);
RCLCPP_INFO(_visualizer_data_node->get_logger(),
"Disconnected with a client");
std::string("Disconnected with a client").c_str());

}

Expand All @@ -136,7 +138,8 @@ void Server::on_message(connection_hdl hdl, server::message_ptr msg)

if (msg->get_payload().empty())
{
RCLCPP_INFO(_visualizer_data_node->get_logger(), "Empty request received");
RCLCPP_INFO(_visualizer_data_node->get_logger(),
std::string("Empty request received").c_str());
return;
}

Expand All @@ -145,15 +148,15 @@ void Server::on_message(connection_hdl hdl, server::message_ptr msg)
if (ok)
{
RCLCPP_DEBUG(_visualizer_data_node->get_logger(),
"Response: %s", response.c_str());
std::string("Response: %s").c_str(), response.c_str());
server::message_ptr response_msg = std::move(msg);
response_msg->set_payload(response);
_server.send(hdl, response_msg);
}
else
{
RCLCPP_INFO(_visualizer_data_node->get_logger(),
"Invalid request received");
std::string("Invalid request received").c_str());
}

}
Expand Down Expand Up @@ -198,7 +201,8 @@ bool Server::parse_request(connection_hdl hdl, server::message_ptr msg,
duration;

RCLCPP_DEBUG(_visualizer_data_node->get_logger(),
"Trajectory Request recived with map_name [%s] and duration [%s]ms",
std::string("Trajectory Request recived with map_name "
"[%s] and duration [%s]ms").c_str(),
request_param.map_name.c_str(), std::to_string(duration_num).c_str());

std::lock_guard<std::mutex> lock(_visualizer_data_node->get_mutex());
Expand Down Expand Up @@ -237,7 +241,7 @@ bool Server::parse_request(connection_hdl hdl, server::message_ptr msg,
else if (j["request"] == "negotiation_trajectory")
{
RCLCPP_DEBUG(_visualizer_data_node->get_logger(),
"Received Negotiation Trajectory request");
std::string("Received Negotiation Trajectory request").c_str());

uint64_t conflict_version = j["param"]["conflict_version"];
std::vector<uint64_t> sequence = j["param"]["sequence"];
Expand Down Expand Up @@ -268,7 +272,8 @@ bool Server::parse_request(connection_hdl hdl, server::message_ptr msg,
catch (const std::exception& e)
{
RCLCPP_ERROR(_visualizer_data_node->get_logger(),
"Error: %s", std::to_string(*e.what()).c_str());
std::string("Error: %s").c_str(),
std::to_string(*e.what()).c_str());
return false;
}

Expand Down Expand Up @@ -369,7 +374,8 @@ std::string Server::parse_trajectories(
catch (const std::exception& e)
{
RCLCPP_ERROR(_visualizer_data_node->get_logger(),
"Error: %s", std::to_string(*e.what()).c_str());
std::string("Error: %s").c_str(),
std::to_string(*e.what()).c_str());
return "";
}

Expand Down
21 changes: 13 additions & 8 deletions rmf_schedule_visualizer/src/VisualizerData.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,9 @@ std::shared_ptr<VisualizerDataNode> VisualizerDataNode::make(

RCLCPP_ERROR(
visualizer_data->get_logger(),
"Mirror was not initialized in enough time ["
+ std::to_string(rmf_traffic::time::to_seconds(wait_time)) + "s]!");
std::string("Mirror was not initialized in enough time ["
+ std::to_string(rmf_traffic::time::to_seconds(wait_time))
+ "s]!").c_str());
return nullptr;
}

Expand Down Expand Up @@ -116,21 +117,23 @@ void VisualizerDataNode::debug_cb(std_msgs::msg::String::UniquePtr msg)
// along with details of trajectories in the schedule
try
{
RCLCPP_INFO(get_logger(), "Mirror Version: [%d]",
RCLCPP_INFO(get_logger(), std::string("Mirror Version: [%d]").c_str(),
data->mirror.viewer().latest_version());
// Query since database was created
auto view = data->mirror.viewer().query(
rmf_traffic::schedule::query_all());
if (view.size() == 0)
RCLCPP_INFO(this->get_logger(), "Schedule is empty");
RCLCPP_INFO(this->get_logger(),
std::string("Schedule is empty").c_str());

else
{
for (const auto& element : view)
{
auto t = element.route.trajectory();
RCLCPP_INFO(
get_logger(), "Trajectory id: [%d]\nTrajectory size: [%d]",
get_logger(),
std::string("Trajectory id: [%d]\nTrajectory size: [%d]").c_str(),
element.route_id, t.size());
int count = 0;
for (auto it = t.begin(); it != t.end(); it++)
Expand All @@ -139,7 +142,8 @@ void VisualizerDataNode::debug_cb(std_msgs::msg::String::UniquePtr msg)
auto finish_time = it->time();
auto finish_position = it->position();
RCLCPP_INFO(get_logger(),
"waypoint: [%d]\ntime: [%s]\nposiiton:[%d, %d, %d]",
std::string("waypoint: [%d]\ntime: i "
"[%s]\nposiiton:[%d, %d, %d]").c_str(),
count,
std::to_string(finish_time.time_since_epoch().count()).c_str(),
finish_position[0], finish_position[1], finish_position[2]);
Expand All @@ -149,7 +153,7 @@ void VisualizerDataNode::debug_cb(std_msgs::msg::String::UniquePtr msg)
}
catch (std::exception& e)
{
RCLCPP_ERROR(this->get_logger(), e.what());
RCLCPP_ERROR(this->get_logger(), std::to_string(*e.what()).c_str());
}
}
}
Expand Down Expand Up @@ -234,7 +238,8 @@ std::vector<Element> VisualizerDataNode::get_negotiation_trajectories(
if (!table_view)
{
RCLCPP_WARN(
this->get_logger(), "table_view for conflict %d not found!",
this->get_logger(),
std::string("table_view for conflict %d not found!").c_str(),
conflict_version);
return trajectory_elements;
}
Expand Down
8 changes: 5 additions & 3 deletions rmf_schedule_visualizer/src/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,8 @@ int main(int argc, char* argv[])

RCLCPP_INFO(
visualizer_data_node->get_logger(),
"VisualizerDataNode /" + node_name + " started...");
std::string("VisualizerDataNode /" + node_name
+ " started...").c_str());

const auto server_ptr = rmf_schedule_visualizer::Server::make(port,
visualizer_data_node);
Expand All @@ -98,13 +99,14 @@ int main(int argc, char* argv[])

RCLCPP_INFO(
visualizer_data_node->get_logger(),
"Websocket server started on port: " + std::to_string(port));
std::string("Websocket server started on port: "
+ std::to_string(port)).c_str());

rclcpp::spin(visualizer_data_node);

RCLCPP_INFO(
visualizer_data_node->get_logger(),
"Closing down");
std::string("Closing down").c_str());

rclcpp::shutdown();
}
25 changes: 14 additions & 11 deletions rmf_schedule_visualizer/src/main_rviz.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,8 @@ class RvizNode : public rclcpp::Node
_rviz_param.start_duration =
std::chrono::seconds(msg->start_duration);
}
RCLCPP_INFO(this->get_logger(), "Rviz parameters updated");
RCLCPP_INFO(this->get_logger(),
std::string("Rviz parameters updated").c_str());
},
sub_param_opt);

Expand All @@ -153,9 +154,9 @@ class RvizNode : public rclcpp::Node
[&](BuildingMap::SharedPtr msg)
{
std::lock_guard<std::mutex> guard(_visualizer_data_node->get_mutex());
RCLCPP_INFO(this->get_logger(), "Received map \""
RCLCPP_INFO(this->get_logger(), std::string("Received map \""
+ msg->name + "\" containing "
+ std::to_string(msg->levels.size()) + " level(s)");
+ std::to_string(msg->levels.size()) + " level(s)").c_str());
// Cache building map message
_map_msg = *msg;
update_level_cache();
Expand Down Expand Up @@ -237,8 +238,8 @@ class RvizNode : public rclcpp::Node
if (!marker_array.markers.empty())
{
RCLCPP_DEBUG(this->get_logger(),
"Publishing marker array of size: " +
std::to_string(marker_array.markers.size()));
std::string("Publishing marker array of size: " +
std::to_string(marker_array.markers.size())).c_str());
_schedule_markers_pub->publish(marker_array);
}
}
Expand Down Expand Up @@ -710,7 +711,8 @@ class RvizNode : public rclcpp::Node
{
_has_level = true;
_level = level;
RCLCPP_INFO(this->get_logger(), "Level cache updated");
RCLCPP_INFO(this->get_logger(),
std::string("Level cache updated").c_str());
publish_map_markers();
publish_floorplan();
break;
Expand All @@ -719,7 +721,8 @@ class RvizNode : public rclcpp::Node

if (!_has_level)
{
RCLCPP_INFO(this->get_logger(), "Level cache not updated");
RCLCPP_INFO(this->get_logger(),
std::string("Level cache not updated").c_str());
}
}
}
Expand All @@ -731,8 +734,8 @@ class RvizNode : public rclcpp::Node

auto floorplan_img = _level.images[0]; // only use the first img
RCLCPP_INFO(this->get_logger(),
"Loading floorplan Image: " + floorplan_img.name +
floorplan_img.encoding);
std::string("Loading floorplan Image: " + floorplan_img.name +
floorplan_img.encoding).c_str());
cv::Mat img =
cv::imdecode(cv::Mat(floorplan_img.data), cv::IMREAD_GRAYSCALE);

Expand Down Expand Up @@ -855,7 +858,7 @@ int main(int argc, char* argv[])

RCLCPP_INFO(
visualizer_data_node->get_logger(),
"VisualizerDataNode /" + node_name + " started...");
std::string("VisualizerDataNode /" + node_name + " started...").c_str());

auto rviz_node = std::make_shared<RvizNode>(
"rviz_node",
Expand All @@ -873,7 +876,7 @@ int main(int argc, char* argv[])
executor.spin();
RCLCPP_INFO(
visualizer_data_node->get_logger(),
"Closing down");
std::string("Closing down").c_str());

rclcpp::shutdown();
}
2 changes: 1 addition & 1 deletion rviz2_plugin/src/DoorPanel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -318,7 +318,7 @@ void DoorPanel::door_state_callback(DoorState::UniquePtr msg)
{
std::string debug_str =
"New door [" + incoming_door_name + "] found, refreshing...";
RCLCPP_INFO(_node->get_logger(), debug_str);
RCLCPP_INFO(_node->get_logger(), debug_str.c_str());
_debug_label->setText(QString::fromStdString(debug_str));
Q_EMIT configChanged();
}
Expand Down
2 changes: 1 addition & 1 deletion rviz2_plugin/src/LiftPanel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -358,7 +358,7 @@ void LiftPanel::lift_state_callback(LiftState::UniquePtr msg)
{
std::string debug_str =
"New lift [" + incoming_lift_name + "] found, refreshing...";
RCLCPP_INFO(_node->get_logger(), debug_str);
RCLCPP_INFO(_node->get_logger(), debug_str.c_str());
_debug_label->setText(QString::fromStdString(debug_str));
Q_EMIT configChanged();
}
Expand Down